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从 20 世纪 60 年 代 以 来 ,机 器 人 技术 的 研究 及 应 用 得 到 了 迅猛 发 展 ,目前 机 器 人 已 经 广 
泛 应 用 于 焊接 喷涂、 搬运 及 装配 等 工业 生产 中 ,并 在 医疗 国防、 家 庭 、 娱 乐 及 特种 应 用 场合 
发 挥 着 重要 作用 。 机 天 人 技术 是 一 门 综合 性 的 工程 学 科 , 涉 及 面 很 宽泛 包括 机 械 学 力学 、 
计算 机 电子 、 信 息 与 控制 等 多 学 科 的 知识 。 本 书 以 作者 在 机 构 学 领域 的 教学 与 科研 成 果 为 
基础 ,借鉴 国内 外 同行 的 经 典 研 究 成 果 ,特别 侧重 于 机 器 人 技术 中 的 运动 学 .动力 学 与 控制 
等 内 容 。 

全 书 共 8 章 :第 1 革 是 机 右 人 技术 基础 ,介绍 机 器 人 技术 发 展 的 历程 ,工业 常用 机 器 人 
的 结构 与 特点 及 基本 概念 等 ;第 2 章 描 述 三 维 空间 中 物体 的 位 置 与 方向 ,以 易于 理解 的 直观 
的 方式 ,建立 机 械 臂 各 部 件 之 间 , 或 者 与 其 周围 空间 的 部 件 、 工 具 之 间 的 位 置 与 方向 的 关系 ， 
是 机 从 人 扩 术 的 基础 和 重点 ;第 3 章 引 入 机 构 运 动 学 中 D-H 方法 ,分 析 机 械 辟 坐标 系 的 建 
立方 法 ,坐标 系 之 间 的 齐 次 变换 矩阵 ,进而 研究 典型 结构 的 机 械 臂 的 正 运 动 学 以 及 道 运动 学 
求解 的 几何 与 代数 法 ;第 4 章 研究 一 般 刚 体 的 速度 问题 ,进而 扩展 到 机 械 臂 速度 雅 可 比 和 矩阵 
的 建立 与 分 析 ; 第 5 章 研究 刚体 的 一 般 运 动 及 螺旋 ,分 析 总 结 刚体 螺旋 运动 和 力 螺旋 表示 形 
式 的 来 龙 去 脉 及 其 蕴含 的 物理 意义 ;第 6 章 研 究 工 业 机 器 人 关节 轨迹 规划 常用 的 基本 方法 ; 
第 7 章 人 研究 工 业 机 龙 人 静 力 学 ,进而 研究 多 刚体 动力 学 牛顿 - 欧 拉 方程 , 拉 格 朗 日 一 类 .二 
类 及 欧 拉 - 拉 格 明日 方程 在 工业 机 器 人 动力 学 建 模 中 的 应 用 ;第 8 章 研 究 工业 机 器 人 的 控 
制 问题 ,包括 独立 关节 控制 与 计算 力矩 控制 。 

本 书 结合 工业 机 融 人 动力 学 模型 的 建立 与 控制 方法 ,编写 了 部 分 仿真 程序 ,为 读者 学 习 
机 大 人 技术 的 控制 方法 与 仿真 提供 了 例 程 。 

本 书 具 体 编写 分 工 如 下 :第 1 ~5 章 由 杨 晓 钧 撰写 ,第 6 ~8 章 由 李兵 撰写 。 硕 士 生 钟 春 
号 绘制 了 书 中 部 分 图 片 ,博士 生 宋 振东 核对 了 书 中 部 分 公式 的 推导 过 程 ,博士 生 王 帅 、 硕 士 
生 刘 俊 伟 做 了 部 分 文字 修订 工作 。 
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第 1 章 绪 de 


1.1 5| 言 


1962 年 ,美国 万 能 自动 化 公司 (Unimation ) 机 器 人 Unimate 在 美国 通用 汽车 公司 (GM) 
投入 使 用 ,标志 着 第 一 代 工 业 机 器 人 的 诞生 。 工 业 机 器 人 作为 集 机 械 . 电 子 控制、 计算 机 、 
传感器 \ 人 工 智 能 等 多 学 科 先 进 技 术 于 一 体 的 现代 制造 业 重要 的 上 自动 化 装备 ,是 当代 科技 发 
展 的 一 个 重要 内 容 。 对 工业 机 器 人 的 定义 ,不 同 的 国家 有 不 同 的 看 法 ,美国 机 需 人 工业 协会 
定义 为 "机 器 人 是 一 种 可 编程 序 的 多 功能 操纵 器 ,被 设计 出 来 用 于 运送 材料 、 零 件 、 工 具 ， 
或 者 作为 专用 设备 在 可 编程 序 控制 下 执行 不 同 的 任务 。 日 本 工业 机 第 人 协会 把 工业 机 衙 
人 定义 为 ”工业 机 器 人 是 一 种 装备 具有 记忆 装置 和 末端 执行 器 的 ,能 够 转动 并 通过 目 动 完 
成 各 种 移动 来 代替 人 类 劳动 的 通用 机 器 。 国际 标准 化 组 织 (ISO) 把 工业 机 器 人 定义 为 : T. 
业 机 器 人 是 一 种 自动 的 \ 位 置 可 控 的 .具有 编程 能 力 的 多 功能 机 械 手 ,这 种 机 械 手 具有 多 个 
轴 ,能 够 借助 于 可 编程 序 来 处 理 各 种 材料 、 零件. 工具 和 专用 装置 ,以 执行 各 种 任务 。 我 国 
的 蒋 新 松 院士 把 工业 机 器 人 定义 为 :“ 一 种 拟人 功能 的 机 械 电 子 六 置 。 

日 本 川崎 重工 于 1967 年 从 美国 Unimation 公司 引进 机 需 人 技术 ,于 1968 年 试制 第 一 台 
川崎 的 Unimate 机 器 人 。1974 年 ,美国 Cincinnati Milaeron 公司 推出 第 一 台 T3 机 器 人 。 
1978 年 日 本 山梨 大 学 的 牧野 洋 提 出 SCARA 机 构 并 完成 实用 的 SCARA ( Selective Compliant 
Articulated Robot for Assembly ) 机 器 人 开发 。1979 年 ,Unimation 公司 推出 可 用 于 装配 的 机 器 
人 PUMA( Programmable University Manipulator), PUMA 机 天 人 的 推出 可 以 看 作 串 联 工业 机 
di A JURA EN as o 

在 机 器 人 学 人 研究 方面 标志 性 成 果 如 下 : 

1954 年 ,Denavit-Hartenberg 提出 表达 空间 杆 件 几 何 关 系 的 一 般 方 法 ,该 方法 用 于 分 析 
机 器 人 正 运动 学 。 

1962 年 ,Emst 和 Boni 分 别 研究 了 带 触 觉 和 压 觉 传感器 的 机 械 手 。 

1964 年 ,Uicker 博士 研究 了 空间 杆 机 构 的 动力 学 问题 。 

1968 年 ,Pieper 用 代数 法 解 逆 运动 学 问题 。 

1972 年 ,Paul 研究 关节 空间 轨迹 规划 问题 。 

1973 年 ,Bolles 和 Paul 在 Stanford Arm 配置 了 视觉 和 力 觉 传 感 副 ,完成 了 水 人 泵 装配 。 

1974 年 ,Bejezy 研究 了 机 占 人 动力 学 和 计算 力矩 控制 方法 。 

1979 年 ,Lozano 和 Wesley 人 研究 了 机 器 人 避 障 问题 。 

其 中 1954 年 和 1962 年 的 成 果 是 工业 机 器 人 技术 运动 学 和 动力 学 的 基础 ,而 当时 的 研 
究 是 面 回 机 构 学 的 。 到 目前 为 止 ,世界 上 各 国 对 工业 机 器 人 的 研究 已 经 历经 了 几 十 年 的 历 
程 ,日 本 美国 法 国 、 德 国 的 工业 机 器 人 产业 日 趋 成 熟 和 完善 ,逐渐 形成 一 批 在 国际 上 较 有 


2 工业 机 器 人 技术 


影响 力 的 知名 的 工业 机 器 人 公司 ,如 日 本 的 安 川 .、OTC、 松 下 FANUC 川崎 等 ;德国 的 KU- 
KA ,CLOOS ;瑞典 的 ABB ;意大利 的 COMAU 及 奥地利 的 IGM o HARA DAEA WRI 
发 和 应 用 有 沈阳 新 松 机 器 人 自动 化 股份 有 限 公 司 ` 四 川 绢 阳 四 维 焊接 上 自动 化 设备 有 限 公司 
等 ,其 中 以 沈阳 新 松 机 器 人 自动 化 股份 有 限 公 司 的 机 大人 产品 最 具 代 表 性 。 


1.2 工业 机 器 人 概述 


1.2.1 机 替 人 系统 的 基本 组 成 


工业 机 妖 人 系统 一 般 由 任务 、 控 制 副 \ 执 行 机构 和 环境 四 个 相互 作用 的 部 分 组 成 ,如 图 
1.1 Bras 


或 者 力 等 反馈 





图 1.1 机 各 人 系统 的 组 成 

(1) 任 务 。 任 务 是 指环 境 的 初始 状态 和 目标 状态 的 差别 。 任 务必 须 用 符合 机 器 人 语言 
规范 要 求 的 程序 语言 来 描述 ,并存 人 机 器 人 的 控制 器 中 ,控制 器 负责 解释 。 

〈2) 控 制 右 。 控 制 器 是 工业 机 如 人 系统 的 大 脑 , 由 计算 机 和 硬件、 软件 及 其 他 控制 电路 组 
成 。 其 中 软件 包括 机 右 人 专用 语言 解释 器 、 机 器 人 运动 学 、 动 力学 及 机 器 人 控制 算法 、 轨 迹 
规划 融 及 任务 调度 ,任务 调度 负责 信息 处 理 及 人 机 交互 ,按照 预存 信息 、 接 收 机 器 人 状态 信 
号 及 环境 信息 ,产生 控制 信号 去 驱动 机 器 人 各 个 关节 运动 。 工 业 机 器 人 控制 器 在 物理 上 分 
为 两 层 , 即 工控 机 与 伺服 控制 锅 ; 在 逻辑 上 一 般 为 三 层 , 即 组 织 层 ,协调 层 和 执行 层 , 如 图 1.2 
所 示 。 

(3) 执 行 机 构 。 执 行 机 构 也 称 机 械 手 、 操 纵 器 或 操作 手 , 一 般 为 多 关节 机 械 结构 ,由 连 
杆 `, 关 节 和 末 问 执行 器 组 成 ,其 末端 执行 器 可 按照 环境 不 同 而 改变 ,如 焊 枪 吸盘 及 手 爪 等 。 

4) 环境 。 环 境 指 机 华人 执行 任务 所 达到 的 几何 空间 内 全 部 事物 自然 特性 所 决定 的 条 
件 。 在 此 环境 中 ,机 器 人 可 能 需要 和 其 他 辅助 装置 共同 完成 任务 ,也 可 能 会 遇 到 障碍 物 和 其 
他 突 发 事件 。 环 境 信息 一 般 是 确定 和 已 知 的 ,这 种 环境 称 为 结构 化 环境 。 在 此 环境 下 ,操作 
人 员 利 用 机 右 人 编程 语言 编制 机 器 人 工作 程序 ,机 器 人 执行 程序 ,可 以 实现 已 知 的 任务 。 在 
许多 情况 下 ,环境 具有 未 知 或 不 确定 性 , 称 为 非 结 构 化 环境 。 在 此 环境 下 ,为 了 实现 不 确定 


外 部 反馈 协调 层 





图 1.2 机 器 人 控制 过 程 


的 任务 ,机 器 人 拥有 外 部 传 感 硕 , 对 工作 对 象 .外 部 环境 具有 一 定 的 感知 能 力 , 例 如 ,外 部 传 
感 器 为 视觉 系统 或 力 传 感 系统 ,获取 的 信息 参与 控制 算法 中 。 

工业 机 器 人 所 具备 的 功能 是 由 其 机 械 部 分 、 传 感 部 分 及 控制 部 分 内 部 集成 所 决定 的 ,但 
是 ,工业 机 器 人 的 作业 能 力 还 取决 于 与 外 部 环境 的 交互 能 力 , 例 如 ,上 下 料 机 需 人 在 与 冲压 
设备 的 交互 中 感知 模具 开局 状态 .工件 饥 送 需 状 态 等 信息 ,与 外 部 设备 的 节 提 协调 一 致 , 实 
现 正确 无 误 的 动作 ,所 以 工业 机 天 人 与 环境 的 交互 是 一 个 重要 问题 。 工 业 机 带 人 与 外 部 环 
境 的 交互 包括 硬件 环境 和 软件 环境 。 与 便 件 环境 的 交互 主要 是 与 外 部 设备 的 通信 、 工 作 域 
中 障碍 和 自由 空间 的 描述 及 操作 对 象 物 的 描述 。 与 软件 环境 的 交互 主要 是 与 生产 单元 监控 
计算 机 所 提供 的 管理 信息 系统 的 通信 。 工 业 机 各 人 不 仅 要 与 已 知 的 定义 了 的 外 部 环境 进行 
交互 ,而 且 有 可 能 面临 变化 的 未 知 的 外 部 环境 。 在 这 种 情况 下 ,工业 机 硕 人 仅仅 实现 可 编程 
控制 是 不 够 的 ,工业 机 器 人 被 引导 去 完成 任务 时 ,在 任何 瞬时 都 要 对 实际 参数 信息 与 所 要 求 
的 参数 信息 进行 比较 ,对 外 部 环境 所 发 生 的 变化 产生 新 的 适应 性 指令 ,实现 其 正确 的 动作 功 
能 ,这 就 是 工业 机 器 人 的 在 线 自 适应 能 力 。 例 如 , 手 部 配置 有 力 传 感 右 的 闻 配 机 融 人 ,在 进 
行 圆 销 插入 作业 时 ,需要 比较 销 与 孔 的 位 置信 息 ,并 且 力 传 感 硕 还 要 获得 儿 配 过 程 中 零件 的 
接触 特征 ,从 而 进行 微量 移动 控制 ,并 利用 手 部 的 柔顺 性 实现 圆 销 插 和 人 作业。 工业 机 天 人 与 
环境 更 高 一 层 的 交互 是 从 外 部 环境 中 感知 、 学 习 、 判 断 和 推理 ,实现 环境 预测 ,并 根据 客观 环 
境 规 划 自 己 的 行动 ,这 就 是 自律 型 机 帮 人 和 智能 化 机 副 人 。 工 业 机 大 人 与 环境 交互 是 机 带 
人 技术 的 关键 ,工业 机 器 人 在 没有 人 工 干 预 的 情况 下 对 外 部 环境 的 目 我 适应 ,行动 的 目 我 规 
划 , 是 机 右 人 技术 及 其 应 用 的 研究 方 呵 。 


1.2.2 工业 机 器 人 第 见 的 结构 
以 串联 机 构 为 构 型 设计 而 成 的 工业 机 器 人 本 体 , 称 之 为 串联 工业 机 器 人 。 它 是 较 早 应 
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用 于 工业 领域 的 机 器 人 ,其 结构 形式 是 杆 件 之 间 通 过 关节 串联 形成 一 个 平面 或 者 空间 开 链 
机 构 , 其 运动 链 长 .负载 能 力 和 刚性 相对 较 差 。 工 业 机 器 人 本 体 包 括 基 座 、 臂 部 和 腕 部 ,大 多 
数 工业 机 器 人 具有 3 ~6 个 驱动 关节 , 腕 部 通常 由 1 ~3 个 驱动 关节 组 成 。 目 前 ,串联 工业 机 
器 人 按照 前 三 个 驱动 关节 的 布置 形式 ,有 五 种 典型 结构 , 即 笛 卡 儿 坐 标 型 .圆柱 坐标 型 球面 
坐标 型 .SCARA 型 和 关节 型 ,主要 应 用 于 焊接 喷涂 、 码 埃 、 包装 、 分 拣 等 领域 。 

1. 笛 卡 儿 坐 标 型 工业 机 器 人 


苗 卡 儿 坐 标 型 机 右 人 (图 1.3) 的 结构 与 数控 机 床 类 似 ,其 工作 空间 内 的 三 个 运动 轴 (x， 
y,z) 由 直线 运动 构成 ,运动 方向 互相 垂直 ,其 末端 执行 器 的 姿态 由 附加 的 旋转 关节 或 机 构 实 
现 。 俏 卡 儿 坐标 型 机 器 人 具有 如 下 优点 :由 工 作 节拍 短 ,能 满足 高 速 的 要 求 ;@ 容 易 与 生产 
线 上 的 传送 带 或 加 工装 配 机 械 相配 合 ;(3 适 于 装 箱 类 、 多 工序 复杂 的 工作 ,定位 容易 改变 ; 
定位 精度 高 ,可 达到 +0.5 mm 以 下 ,载重 发 生变 化 时 不 会 影响 精度 ;@ 易 于 控制 。 其 缺点 
是 机 构 体 积 庞 大 ,工作 空间 与 设备 体积 比 小 ,操作 灵活 性 较 差 。 





图 1.3 Epson 笛 卡 儿 型 机 器 人 及 结构 布置 示意 图 


2. 圆柱 坐标 型 工业 机 器 人 

圆柱 坐标 型 工业 机 占 人 在 基 座 水 平 转台 上 装 有 立柱 ,水 平 臂 可 沿 立 柱 做 上 下 运动 并 可 
在 水 平方 向 伸缩 ,其 末端 工作 空间 形成 一 个 圆柱 面 , 称 为 圆柱 坐标 型 工业 机 器 人 。 它 适用 于 
搬运 和 测量 工件 ,具有 直观 性 好 结构 简单 .本体 占 用 空间 较 小 等 特点 。 这 种 结构 方案 的 优 
点 是 末端 执行 器 可 获得 较 高 的 速度 ,缺点 是 末端 执行 器 外 伸 离 开 立 柱 轴 心 越 远 ,其 定位 精度 
越 低 。 图 1.4 所 示 为 一 款 典 型 圆柱 坐标 型 工业 机 器 人 。 

3. SCARA 型 工业 机 器 人 


图 1.5 所 示 为 选择 顺应 性 装配 工业 机 器 人 (Selective Compliant Articulated Robot for As- 
sembly , SCARA ) ,是 一 种 特殊 类 型 的 圆柱 坐标 型 机 器 人 ,通常 该 机 器 人 具有 两 个 转动 一 个 
平 动 及 一 个 转动 , 共 四 个 驱动 关节 ,末端 具有 四 个 运动 自由 度 ( 包 括 x,y,z 方向 的 平 动 自由 
度 和 绕 z 轴 的 转动 自由 度 )。SCARA 工业 机 器 人 在 x*,y 方 向 上 具有 顺从 性 ,而 在 z 轴 方 向 具 
有 良好 的 刚度 ,此 特性 特别 适合 于 装配 工作 , 故 SCARA 工业 机 器 人 首先 大 量 用 于 装配 印刷 
电路 板 和 电子 零 部 件 。SCARA 工业 机 器 人 的 另 一 个 特点 是 其 串联 的 平面 2R 机 构 , 类 似 人 
的 手臂 ,可 以 伸 进 有 限 空间 中 作业 ,然后 收回 ,适合 于 搬 动 和 取 放 物件 。 
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K| 1.5 Epson SCARA 工业 机 器 人 及 结构 布置 示意 图 


4. 球面 坐标 型 工业 机 器 人 

球面 坐标 型 工业 机 融 人 驱动 关节 的 布置 包括 两 个 转动 关节 和 一 个 移动 关节 ,未 端 执行 
仑 可 以 在 水 平面 内 转动 ,也 可 以 在 垂直 面 内 摆动 ,并 且 可 以 做 里 外 伸缩 ,其 工作 空间 形成 球 
面 的 一 部 分 , 称 为 球面 坐标 型 工业 机 器 人 。 图 1.6 为 一 款 球面 坐标 型 工业 机 器 人 。 
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图 1.6 Stanford 球面 坐标 型 工业 机 器 人 及 结构 布置 示意 图 
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5. 关节 型 工业 机 器 人 

关节 型 工业 机 器 人 的 结构 类 似 人 的 腰部 和 臂 部 ,其 末端 位 置 和 姿态 全 部 由 转动 天 忆 实 
现 , 如 图 1.7 所 示 。 其 优点 是 机 构 紧 凑 , 灵 活性 好 , 占 地 面积 小 ,工作 空间 大 ,可 获得 较 高 的 
未 端 操 作 器 线 速度 ;其 缺点 是 运动 学 模型 复杂 ,高 精度 控制 难度 大 ,空间 线 位 移 分 辨 率 取决 
于 机 器 人 手臂 的 位 姿 。 





图 1.7 ABB 关节 型 工业 机 器 人 及 结构 布置 示意 图 
目前 ,全 球 现 有 的 工业 机 器 人 中 有 2296 为 笛 卡 儿 坐 标 型 工业 机 峰 人 ,13 为 SCARA 型 
工业 机 器 人 ,4% 为 圆柱 坐标 型 工业 机 器 人 ,而 超过 60% 为 关节 型 工业 机 器 人 ,关节 结构 已 
逐渐 成 为 工业 机 屁 人 的 通用 结构 。 


1.3 基本 概念 及 其 定义 


1.3.1 机 如 人 的 自由 度 与 活动 度 


刚体 相对 于 参考 坐标 系 进行 独立 运动 的 数目 为 刚体 的 自由 度 ( Degree of Freedom, 
DOF) 。 在 笛 卡 儿 坐 标 系 下 ,刚体 能 够 进行 的 运动 为 治 绕 三 个 坐标 轴 平 移 运动 或 者 旋转 运 
动 ,意味 痢 能 够 运用 三 个 平移 和 三 个 旋转 参数 描述 刚体 相对 于 参考 坐标 系 的 位 置 和 方向 。 
空间 自由 刚体 有 六 个 自由 度 , 当 两 个 刚体 确立 某 种 关系 时 ,刚体 之 间 由 于 约束 的 存在 ,将 失 
去 一 些 自由 度 。 

(1) 机 天 人 的 目 由 度 。 机 坑 人 的 目 由 度 是 指 末端 杆 件 或 者 执行 希 相 对 于 参考 坐标 系 独 
立 运 动 的 数目 ,通常 机 械 手 臂 由 互相 连接 的 刚体 组 成 , 奇 要 求 机 器 人 能 够 到 达 任 务 空间 的 任 
意 位 置 和 姿态 ,应 具有 六 个 自由 度 。 

(2) 机 大 人 的 活动 度 。 机 器 人 的 活动 度 ( Degree of Mobility ) 是 指 机 器 人 所 有 驱动 关节 
具有 的 目 由 运动 的 数目 ,在 平面 内 运动 的 机 器 人 ,其 驱动 关节 可 能 多 于 三 个 ,但 在 任何 情况 
下 ,此 人 台 机 右 人 的 自由 度 不 多 于 三 个 。 在 三 维 空间 下 机 器 人 最 多 具有 六 个 自由 度 ,但 可 以 有 
较 多 的 活动 度 ,活动 度 越 多 ,机 器 人 灵活 性 越 大 ,控制 难度 也 将 增加 。 


1.3.2 运动 副 或 关节 
一 般 而 言 ,机 械 臂 是 由 杆 件 通过 运动 副 依 次 连接 而 成 的 机 构 。 杆 件 是 组 成 机 构 的 基本 
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运动 单元 ,可 以 是 一 个 零件 ,也 可 以 由 若干 零件 刚性 连接 在 一 起 组 成 。 当 由 杆 件 组 成 机 构 
时 ,需要 以 一 定 的 方式 把 各 个 杆 件 依次 连接 起 来 ,每 个 杆 件 至 少 与 为 外 一 个 杆 件 连接 ,连接 
不 是 刚性 的 ,应 保证 彼此 连接 的 两 个 杆 件 产生 某 些 相对 运动 。 由 两 个 杆 件 组 成 的 可 动 的 连 
接 称 为 运动 副 ,在 工业 机 器 人 机 构 中 形象 地 称 为 关节 (Joint) 。 币 用 运动 副 的 简 图 、 代 号 及 
自由 度数 见 表 1.1。 机 构 常 以 它 所 含 的 全 部 运动 副 的 代号 来 命名 ,例如 ,从 基 座 开始 ,一 个 
转动 副 和 两 个 移动 副 组 成 的 机 构 ,例如 图 1.4 所 示 机 构 , 通 常 称 为 RPP 机 构 。 
RLI 常见 运动 副 的 简 图 、 代 号 及 自由 度 
运动 副 简 图 代号 ”自由 度 | 运动 副 简 图 代号 Bu 
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1.3.3 运动 链 

两 个 以 上 的 杆 件 通过 运动 副 的 连接 而 构成 的 系统 称 为 运动 链 ( Kinematics Chain) 。 如 
果 运 动 链 的 杆 件 未 构成 首尾 封闭 的 结构 , 称 为 开 环 (Open-loop ) 运动 链 ,简称 开 链 。 如 果 运 
动 链 的 各 杆 件 构成 首尾 封闭 的 结构 , 称 为 闭环 (Close-loop) 运 动 链 , 简称 闭 链 。 在 运动 链 
中 ,如 果 每 个 杆 件 都 在 同一 平面 上 ,或 者 在 相互 平行 的 平面 内 运动 , 则 称 为 平面 运动 链 , 否 则 
称 为 空间 运动 链 。 


1.3.4 机 构 


在 运动 链 中 ,如 条 以 茶 一 杆 件 为 参考 系 , 当 某 些 杆 件 按 给 定 的 运动 规律 运动 时 ,其 余 所 
有 杆 件 都 有 确定 的 相对 运动 ,那么 此 运动 链 便 成 为 一 个 机 构 。 机 构 中 作 参 考 系 的 杆 件 称 为 
HLI , 按 给 定 运动 规律 运动 的 杆 件 称 为 原 动 件 ,其余 的 称 为 从 动 件 。 从 动 件 的 运动 规律 取决 
于 原 动 件 的 运动 规律 和 机 构 的 结构 形式 。 多 于 一 个 闭 链 的 机 构 称 为 多 闭 链 机 构 或 多 环 机 
构 , 只 有 一 个 闭 链 的 机 构 称 为 单 闭 链 机 构 。 
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1.3.5 笛 卡 儿 坐 标 系 和 广义 坐标 


考虑 一 个 具有 个 质点 的 系统 ,1S} = {P,P,,…,P,1 ,在 笛 卡 儿 坐标 系 下 ,第 i 个 点 的 

EREJE T, r(x Yi) ,可 以 表示 为 
r,-xüity,;jozk,021,2,:,n CL t) 
n 个 质点 的 系统 需要 3n 个 物理 坐标 来 确定 其 位 置 ,在 许多 情况 下 ,为 了 方便 分 析 系 统 
的 运动 ,使 用 一 组 相互 独立 的 变量 集合 9 ,9 ,…,dw 来 表示 ,其 物理 坐标 为 
x,7x1(41,05, 777,04) 
y ey.(41,42 777,45) 
z,722,(41,05, 7,04) 
(1.2) 
X, 2X,(41,42, 777,44) 
Ys 7 Y (415425777504) 
Zn 72,(Q1,42 77,44) 
则 qi ,q;，,… ,qm 为 广义 坐标 ,可 以 完整 地 、 唯 一 地 摘 述 动态 系统 的 位 置 。 一 般 而 言 ,一 个 机 
构 的 运动 可 由 m 个 相互 独立 的 广义 坐标 来 描述 ,系统 的 任意 位 形 都 可 以 用 这 m 个 广义 坐标 
来 表示 。 广 义 坐 标 必须 是 相互 独立 的 ,任何 一 组 能 够 完全 描述 系统 运动 的 独立 变量 都 可 以 
选 作 广义 坐标 ,可 以 是 篆 卡 儿 坐 标 , 也 可 以 是 其 他 量 , 如 极 坐 标 、 球 坐标 .相对 角 位 移 或 线 位 
移 。 
【 例 1.1】 如 图 1.8 所 示 的 平面 2R 机 构 , 杆 1 的 长 度 为 由 , 绕 z 轴 的 转动 角 为 qi; 杆 2 
的 长 度 为 i, ,相对 于 杆 1 的 转动 角 为 9。 杆 2 绕 z 轴 的 转动 角 为 p, 杆 2 上 点 己 在 笛 卡 儿 坐 
标 系 下 的 位 置 和 拓 量 p(x,,y,) 可 以 表示 为 
p=qı tqz 
x, 2l, cos q, +l, cos q, +q, (1.3) 
y, 7l, sin q, tL sin(q,*q,) 

式 中 ,gi ,qs 是 描述 运动 的 独立 参数 ,确定 一 个 系统 的 运动 状况 ,为 系统 的 广义 坐标 。 

1.3.6 位 形 空间 

1. 连 杆 的 位 姿 ( Position and Orientation ) 

连 杆 的 位 置 和 姿态 可 以 用 与 连 杆 固 连 的 坐标 系 相 对 于 参考 坐标 的 位 置 和 姿态 表示 ,如 


图 1.9 所 示 ,平面 运动 的 连 杆 的 位 置 和 姿态 x. 可 以 给 定 为 
x, - (r,R,) (1.4) 


式 中 ,r 表示 坐标 系 |1| 原点 相对 于 参考 系 10| 的 位 置 ,r= ;R, 表示 坐标 系 |1| 相对 |0} 的 


cos 0 -sin 0 


方向 矩阵 ,R= pa 
的 角度 。 


| 。 位 置 和 姿态 也 可 以 给 定 为 (*,y,0) ,0 表示 连 杆 绕 = 轴 转动 


cos 0 


21* S — x 9 





图 1.8 平面 2R 机 构 图 1.9 平面 单 杆 的 位 置 与 姿态 
2. 机 构 位 形 ( Mechanism Configuration ) 
机 构 位 形 是 指 在 符合 关节 装配 约束 的 条 件 下 ,机构 运 动 瞬时 各 连 杆 的 位 姿 组 成 ,以 平面 
四 杆 机 构 为 例 ,图 1. 10 所 示 为 平面 四 杆 机 构 的 两 种 位 形 。 


图 1.10 平面 四 杆 机 构 的 两 种 位 形 

(1) 转 动 关节 的 装配 约束 。 假 定 有 两 个 连 杆 石和 六 ,如 图 1. 11 所 示 , 两 者 通过 转动 关 
节 相 连 ,此 连接 相当 于 杆 五 上 点 P; 和 杆 LE @; 点 重合 ,在 连 杆 石和 万 上 分 别 建立 固 连坐 标 
系 FF 和 FF, 有 以 下 约束 条 件 

r,* Rip; -r,*R,"*q, (1.5) 
A,r ,ri 分 别 表示 坐标 系 FF 和 原点 在 参考 坐标 系 Fi 下 的 位 置 矢量 ;R,,R, 为 坐标 系 F, 
和 Fi 相对 于 参考 坐标 系 FF 的 方向 矩阵 ;ip;, q: 为 点 P;: 和 0Q,; 分 别 在 坐标 系 F 和 Fi 下 的 位 置 
矢量 。 

(2) 移动 关节 的 装配 约束 。 假 定 有 两 个 连 杆 五 和 六 ,两 者 通过 移动 关节 相连 ,如 图 1. 12 
所 示 , 此 连接 为 初始 状态 时 , 杆 L 上 的 点 P; 和 杆 LEKS Q; 重 合 , 随 着 移动 关节 的 平 动 , 在 
关节 轴线 方向 e; 上 两 点 之 间 的 距离 为 d;, 有 以 下 约束 条 件 : 

r+R ip;=r,+R,( 9;+di *e;) 
R,=RR, 


式 中 ,Rj 为 坐标 系 Fi 相对 于 坐标 系 Fj 的 方向 矩阵 。 


(1. 6) 
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图 1.11 转动 关节 约束 关系 图 1.12 移动 关节 约束 关系 
(3) 机 架 。 通 常 机 架 标 记 为 杆 0, 固 连坐 标 系 Ff 与 参考 坐标 系 重合 。 此 时 


lol elo i 


(4) 位 形 空间 隐 式 表示 (Implicit Representation) 。 个 平面 连 杆 的 机 构 位 姿 坐 标 天 量 为 


ind lE TL T ii a h (3 
所 有 关节 装配 约束 条 件 为 
DCq)=0 (1. 8) 
则 位 形 空间 (C-space ) 集合 为 
C-1q:0(q)-0] [11,93 


这 是 位 形 空 间 的 隐 式 表示 ,坐标 矢量 g 中 各 分 量 可 能 会 相互 依赖 ,必须 满足 约束 条 件 
P(gq)=0,。 

【 例 1.2】 平面 2R 机 械 臂 如 图 1. 13 所 示 , 坐标 系 固 连 于 杆 0 ,为 参考 坐标 系 , 分 别 
在 杆 1 和 杆 2 的 前 并 建立 固 连 坐标 系 了 ,和 F, 杆 1 和 杆 2 的 长 度 分 别 为 LL 和 4, 试 描述 2R 
机 械 臂 的 位 形 空间 。 





杆 0 关节 1 
图 1.13 2R 机 械 臂 位 形 描述 
首先 给 出 杆 1 和 杆 2 的 坐标 矢量 为 
q-|2,,y1,0,,x; y; AI 
依据 公式 (1.5) ,关节 1 的 装配 约束 为 
=0 ET. 10) 
X 2 的 装配 约束 为 


;] 2^ (1.11) 


F 
r,*R, 





结合 式 (1. 10) (1. 11) ,得 出 的 约束 条 件 为 
P(gq)=0 
多 =0 
y1=0 (1. 12) 
l cos 0,—x;, 0 
l sin 0, -y, 0 
(5) 位 形 空间 参数 表示 方法 (Parametric Representation), 4 n 杆 机 械 臂 是 各 杆 件 通过 
关节 依次 串联 而 成 的 开 链 结构 时 ,能 找 出 独立 的 关节 坐标 矢量 g ,可 以 完整 描述 机 构 的 瞬时 
位 形 。 与 式 (1.9) 相 比 , 其 位 形 空 间 易 于 理解 和 表示 ,任何 连 杆 的 位 姿 可 以 用 4 显 式 表示 ， 
称 为 参数 表示 方法 。 图 1. 8 所 示 的 平面 2R 机 构 位 形 参 数 表示 法 为 
q-[9.41 
参数 表示 法 并 不 是 一 直 存在 的 ,对 于 闭 链 机 构 仅 能 通过 隐 式 形式 表示 位 形 。 


1.3.7 工作 空间 


机 恬 人 未 端 执行 融和 运 动 描述 参考 点 所 能 达到 的 空间 点 的 集合 , 称 为 机 器 人 工作 空间 

( Work Space) 或 任务 空间 (Task Space) o BL28 A. TZ [8] X. HT 4847 A 2c TE ( Dexterous ) 工作 

空间 和 可 达 (Reach Able) TZE. 4H] FH — à , Bas AR EAEE XS 2525] n] $1] 35 ,此 类 点 

组 成 的 空间 称 为 柔性 工作 空间 。 机 器 人 至 少 以 一 种 姿态 到 达 的 点 组 成 的 空间 称 为 可 达 工 作 

空间 。 显 然 , 可 达 工 作 空间 大 于 柔性 工作 空间 。 机 需 人 工作 空间 一 般 用 水 平面 和 垂直 面 的 
投影 表示 。 


1.3.8 额定 速度 与 额定 负载 


机 化 人 未 闪 的 运动 是 机 右 人 所 有 关节 运动 的 合成 ,对 应 于 末端 运动 的 一 段 轨迹 ,相应 的 
各 关节 运动 过 程 一 般 包括 局 动 \. 加 速 、 勺 速 、 减 速 来 完成 。 为 了 提高 生产 效率 , 硕 望 机 顺 人 运 
动 周期 短 ,那么 局 动 , 加 速 及 减速 时 间 要 尽 可 能 短 ,但 过 大 的 加 减速 会 造成 较 大 的 惯性 ,容易 
出 现 振动 ,运行 不 平稳 。 因 此 ,机 需 人 在 保持 运动 平稳 及 位 置 精度 的 前 提 下 所 能 达到 的 最 大 
速度 称 为 额定 速度 ( Rated Velocity) 。 各 关节 的 运动 速度 称 为 单 轴 速 度 , 其 末端 的 速度 是 由 
各 关节 的 速度 合成 。 在 额定 速度 和 行程 范围 内 ,末端 执行 句 所 能 承受 负载 的 允许 值 称 为 额 
定 负载 (Rated Load)。 超 大 型 机 融 人 ,负载 能 力 在 1 000 kg 以 上 ;大 型 机 器 人 ,负载 能 力 在 
100 ~ 1 000 kg; 中 型 机 右 人 ,负载 能 力 为 10 ~ 100 kg; 小 型 机 人 右 人 ,负载 能 力 为 0.1~ 10 kg; 
超 小 型 机 大 人 ,负载 能 力 在 0.1 kg 以 下 。 


1.3.9 精度 


Art Control Resolution) 是 机 器 人 各 关节 能 够 实现 的 最 小 移动 距离 或 最 小 
半角。 末端 控制 分 辩 率 是 机 龙 人 控制 占 根 据 指令 能 控制 的 最 小 位 移 增 量 。 如 图 1. 14 所 示 ， 
电机 通过 减速 箱 带 动 丝 杠 运动 ,进而 驱动 托 板 移动 ,假定 电机 的 编码 器 为 二 进 制 ,位 数 为 m, 


12 工业 机 器 人 技术 


减速 箱 的 减速 比 为 了 =mym , 丝 杠 的 导 程 为 dg, 则 此 关 市 的 控制 分 辨 率 Ci 为 
(1.13) 





图 1.14 单 关节 移动 分 辨 率 

从 式 (1. 13) 可 以 看 出 ,对 于 图 1. 14 所 示 的 传动 方式 ,提高 关节 分 辨 率 的 方法 可 以 是 加 
大 减速 比 或 选择 大 位 数 的 编码 项 。 对 于 移动 关节 ,此 关节 的 分 辨 率 传递 到 末端 保持 不 变 , 而 
对 于 转动 关节 ,其 控制 角 分 辨 率 需要 乘 以 臂 长 得 到 末端 的 分 辨 率 。 

末端 空间 分 辨 率 ( Spatial Resolution) 是 指 末端 执行 器 运动 的 最 小 增 量 ,其 大 小 受 包 括 末 
端 控制 分 辨 率 、 机 械 误差 及 计算 机 控制 计算 误差 等 在 内 多 因素 影响 。 

(1) 定 位 精度 与 重复 定位 精度 。 机 器 人 执行 一 位 姿 指 令 ,其 末端 执行 器 在 参考 坐标 系 
下 实际 到 达 位 置 和 指令 位 姿 之 间 的 偏差 称 为 位 姿 误 差 (Accuracy)。 位 姿 误 差 又 可 细 分 为 位 
置 误差 (Position Accuracy ) 和 姿态 误差 (Orientation Accuracy)。 在 相同 条 件 下 ,机 器 人 多 次 
执行 同一 位 姿 指 令 , 所 测量 得 到 实际 位 姿 分 布 的 不 一 致 程度 , 称 为 重复 定位 精度 ( Repeat- 
ability) 。 在 茶 一 时 间 段 或 者 不 同 温 度 条 件 下 ,能 保持 相同 测量 特性 的 能 力 , 称 为 稳定 性 。 
假定 机 人 逢 人 目标 指令 的 位 置 为 图 中 心 点 的 位 置 , 执行 四 次 指令 实际 到 达 的 位 置 如 图 
1. 15(a) Brzn ,每 次 距离 中 心 点 位 置 较 远 ,并 且 四 次 实际 位 置 分 布 分 散 ,表明 机 器 人 定位 精 
度 和 重复 定位 精度 较 差 ; 同 理 ,图 1.15(b) 表 明 机 器 人 具有 较 差 的 定位 精度 和 良好 的 重复 定 
位 精度 ;图 1. 15(c) 表 明 机 器 人 具有 良好 的 定位 精度 与 良好 的 重复 定位 精度 。 





(a) 低 定位 精度 与 低 (b6) 较 差 的 定位 精度 与 良好 (c) 良好 的 定位 精度 与 良好 
重复 定位 精度 的 重复 定位 精度 的 重复 定位 精度 


图 1. 15 定位 与 重复 定位 精度 示意 图 

(2) 线 性 误差 。 如 有 果 名 义 输入 输出 是 一 种 线性 关系 ,如 图 1. 16 所 示 , 则 测量 实际 输出 
与 名 义 输出 最 大 的 偏差 与 输出 范围 的 百分比 定义 为 线性 误差 。 

(3) 偏 置 误差 。 偏 置 误差 指 零 输入 时 ,输出 实际 值 与 名 义 值 的 差 值 。 在 机 器 人 复位 完 
成 后 ,各 关节 实际 位 置 与 控制 系统 内 的 名 义 位 置 存 在 偏差 ,此 误差 对 机 器 人 未 端 定位 精度 的 
影响 较 大 。 如 图 1. 17 所 示 ,为 平面 2R 机 械 臂 的 初始 位 形 ,假定 第 1 驱动 轴 电 机 与 关节 之 间 
减速 系统 的 减速 比 为 了 ,名 义 上 ,此 时 关节 位 置 为 90", 名 义 上 ,电机 转角 输入 gw 与 关节 位 
置 的 输出 gwu 之 间 的 关系 为 
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Op = NO inga 907 (1. 14) 
当 bu=0 时 ,第 1 关节 的 名 义 位 置 为 90" ,但 实际 上 ,当前 名 义 位 置 与 实际 位 置 存在 偶 
差 , 因 为 工业 机 器 人 壁 展 较 长 ,各 个 关节 的 偏 置 误差 对 末端 定位 精度 影响 较 大 。 





输 
出 
初始 位 形 
6.=90° 
输入 范围 x， 
图 1.16 线性 误差 示意 图 图 1.17 2R 机 械 臂 初始 位 形 


本 章 小 结 


本 章 介 绍 了 工业 机 器 人 系统 的 基本 组 成 ,串联 工业 机 堪 人 篆 见 的 结构 构 型 及 其 特点 。 
重点 前 述 了 本 书后 面 章 节 将 用 到 的 基本 概念 及 常用 术语 ,为 后 续 的 学 习 现 定 基 础 。 
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对 机 器 人 运动 进行 描述 ,不 仅 涉及 机 器 人 本 体 , 而 且 还 涉及 空间 各 物体 间 以 及 物体 与 机 
髓 人 基 上 坐标 系 之 间 的 关系 。 本 草 首 先 介 绍 机 人 副 人 常用 的 坐标 系 ,然后 讨论 两 坐标 系 之 间 位 
置 与 姿态 的 描述 ,进而 通过 齐 次 变换 矩阵 描述 空间 相同 的 点 在 不 同 坐标 系 之 间 的 映射 。 最 
后 ,用 齐 次 变换 矩阵 描述 刚体 在 空间 的 平移 与 旋转 运动 ,对 于 刚体 的 旋转 和 运动 ,详细 讨论 用 
欧 拉 角 方 式 .一 般 旋 转 方式 以 及 欧 拉 - 罗 德里 格 斯 参数 方式 描述 刚体 的 空间 旋转 运动 。 


2.1 HLEA EER RA 


机 和 侣 人 程序 中 所 有 点 的 位 置 都 是 和 一 个 坐标 系 相 联 系 的 ,同时 ,这 个 坐标 系 也 可 能 和 田 
外 一 个 坐标 系 有 联系 。 机 大人 系统 第 用 的 坐标 系 有 如 下 几 种 。 
1. ÆRA ( Base Coordinate System ) 


ERER BI Dr FE n , RT EUE D Ud A EA b Zh UE , BERRI z RULA 1 关 
TET. HAARR EWK 2. 1 所 示 ,一 般 而 言 ,原点 位 于 第 1 关节 轴 轴 线 和 机 
俘 人 基础 安 疫 平面 的 交点 ,并 以 基础 安装 平面 为 xy 平面 ,x 轴 方 向 指 问 正 前 方 ,y 轴 与 x 轴 
符合 右手 法 则 ,z 8 E. 





图 2.1 Lar A EAS 


2. 世界 坐标 系 ( World Coordinate System ) 


AIR Li A Eze T Th , ERR ERARE o PA TU, A Dti AN mH , HLA 
人 末端 移动 直观 性 差 , 因 而 示 教 编程 较为 困难 。 男 外 ,如 果 两 台 或 更 多 台 机 器 人 共同 协作 完 
成 一 项 任务 时 ,例如 ,一 台 安 狼 于 地 面 , 另 一 台 倒 置 ,倒置 机 器 人 的 基 坐 标 系 也 将 上 下 颠倒 。 
如 有 末 分 别 在 两 人 台 机 磊 人 的 基 坐 标 系 中 进行 运动 控制 , 则 很 难 预测 相互 协作 运动 的 情况 。 在 
此 情况 下 ,可 以 定义 一 个 世界 坐标 系 ,选择 共同 的 世界 坐标 系 取而代之 。 知 无 特殊 说 明 , 单 
台 机 和 硕 人 世界 坐标 系 和 基 坐 标 系 是 重合 的 。 如 图 2.2 所 示 , 当 在 工作 空间 内 同时 有 几 台 机 
fit AEST ,使 用 公共 的 世界 坐标 系 进 行 编程 有 利于 机 器 人 程序 间 的 交互 。 
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A 一 基 坐 标 系 ;B 一 基 坐 标 系 ;C 一 世界 坐标 系 
图 2.2 ”世界 坐标 系 


3. APERA (User Coordinate System ) 


机 器 人 可 以 和 不 同 的 工作 台 或 夹具 配合 工作 ,在 每 个 工作 台 上 建立 一 个 用 户 坐 标 系 。 
机 需 人 大 部 分 采用 示 教 编程 的 方式 ,步骤 烦琐 ,对 于 相同 的 工件 ,如 果 放 置 在 不 同 的 工作 人 台 
上 ,在 一 个 工作 人 台 上 完成 工件 加 工 示 教 编程 后 ,如 果 用 户 的 工作 台 发 生变 化 ,不必 重 新 编程 ， 
只 需 相 应 地 变换 到 当前 的 用 户 坐 标 系 下 。 用 户 坐 标 系 是 在 基 坐 标 系 或 者 世界 坐标 系 下 建立 
的 。 如 图 2. 3 所 示 ,用 两 个 用 户 坐标 系 来 表示 不 同 的 工作 平台 。 







用 户 举 标 系 2 PELLI 
3 ESSE PREDA: 
一 e A 

基 举 标 系 HIP MEER IR 1 -一 一 一 一 


图 2.3 ”用户 坐标 系 
4. 工件 坐标 系 (Object Coordinate System ) 


用 户 坐 标 系 是 用 来 定义 不 同 的 工作 人 台 或 者 夹具 的 。 然 而 ,一 个 工作 台 上 也 可 能 放 着 几 
个 需要 机 第 人 进行 加 工 的 工件 ,所 以 和 和 定义 用 户 坐 标 系 一 样 ,也 可 以 定义 不 同 的 工件 坐标 
系 , 当 机 帮 人 加 工 工作 台 上 加 工 不 同 的 工件 时 ,只 需 变换 相应 的 工件 坐标 系 即 可 。 工 件 坐 标 
系 是 在 用 户 坐 标 系 下 建立 的 ,两 者 之 间 的 位 置 和 姿态 是 确定 的 。 如 图 2.4 所 示 ,表示 在 同一 
个 工作 人 台 上 的 两 个 不 同 工 件 , 它 们 分 别 用 两 个 不 同 的 工件 坐标 系 表示 。 例 如 ,在 工件 坐标 系 
下 待 加 工 的 轨迹 点 ,可 以 变换 到 用 户 坐 标 系 下 ,进而 变换 到 基 坐 标 系 下 。 

5. 置换 坐标 系 ( Displacement Coordinate System ) 


有 时 需要 对 同一 个 工件 .同一 段 轨 迹 在 不 同 的 工 位 上 加 工 , 为 了 避免 每 次 重新 编程 ,可 
以 定义 一 个 置换 坐标 系 。 置 换 坐标 系 是 基于 工件 坐标 系 定义 的 。 如 图 2.5 所 示 , 当 置 换 坐 
标 系 被 激活 后 ,程序 中 的 所 有 点 都 将 被 置换 。 在 RAPID 语言 中 ,有 三 条 指令 (PDispSet， 
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图 2.4 工件 坐标 系 
PDispOn, PDispOff) 关系 到 置换 坐标 系 的 使 用 。 


置换 坐标 系 


工件 坐标 系 
图 2.5 置换 坐标 系 
6. 腕 坐标 系 ( Wrist Coordinate System) 


腕 坐标 系 和 下 面 的 工具 坐标 系 都 是 用 来 定义 工具 的 方向 的 。 在 简单 的 应 用 中 , 腕 坐标 
系 可 以 定义 为 工具 坐标 系 , 腕 坐标 系 和 工具 坐标 系 重合 。 腕 坐标 系 的 z 轴 和 机 器 人 的 第 6 
根 轴 重 合 , 如 图 2.6 所 示 ,坐标 系 的 原点 位 于 末端 法 兰 盘 的 中 心 ,x 轴 的 方向 与 法 兰 盘 上 标 
识 孔 的 方 回 相同 或 相反 ,z 轴 垂 直 回 外 ,y 轴 符 合 右手 法 则 。 


Z 


腕 坐标 系 hy 
图 2.6 胶 坐 标 系 
7. 工具 坐标 系 ( Tool Coordinate System ) 


安装 在 末端 法 兰 盘 上 的 工具 需要 在 其 中 心 点 (TCP) 定义 一 个 工具 坐标 系 , 通 过 坐标 系 
的 转换 ,可 以 操作 机 器 人 在 工具 坐标 系 下 运动 ,以 方便 操作 。 如 果 工 具 磨 损 或 更 换 , 只 需 重 
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新 定义 工具 坐标 系 ,而 不 用 更 改 程序 。 工 具 坐 标 系 建立 在 腕 坐标 系 下 , 即 两 者 之 间 的 相对 位 
置 和 姿态 是 确定 的 。 图 2. 7 所 示 表 示 不 同 工 具 的 工具 坐标 系 的 定义 。 





sams 
—H i 


(a) 3IUSU& A^ bs 7 z (b) 点 焊 枪 坐标 系 


一 





图 2.7 工具 坐标 系 
8. 关节 坐标 系 ( Joint Coordinate System ) 
KTERAK EULA RET 5 H)38 3/] , lE 2. 8 所 示 。 所 有 关节 类 型 可 能 
不 同 ( 如 移动 关节 SISSE). BER HLAS A R Jm T9 27 $188 28 BS] fr BL, VU TE IRI AS 
A FERME, nI LUKE X 352 , Attis FLAA R m $3518 E HJ EG 





图 2.8 关节 坐标 系 
EREDA A HERR ,那么 在 不 同 的 坐标 系 下 ,空间 相同 的 一 个 点 是 如 何在 不 同 
坐标 系 下 映射 的 呢 ? 例如 ,工件 上 的 点 如 何 映射 到 基 坐 标 系 下 表示 ,或 者 在 工具 坐标 系 下 表 
示 。 下 面 章节 将 讨论 这 个 问题 。 


2.2 位 置 和 姿态 的 表示 


机 髓 人 运动 学 研究 通常 涉及 多 物体 之 间 空 间 位 置 与 姿态 的 关系 ,如 机 械 手 、 工 具 和 工件 
等 。 为 了 确定 各 物体 之 间 的 位 置 与 姿态 ,需要 建立 与 之 固 连 的 坐标 系 ,通过 分 析 各 坐标 系 之 
闻 的 位 置 与 姿态 ,进而 确定 各 物体 之 间 的 位 置 与 姿态 的 关系 。 本 书 首先 从 两 个 坐标 系 之 间 
的 位 置 与 姿态 分 析 和 人 手 ,进一步 可 以 推演 到 多 坐标 系 之 间 的 位 置 与 姿态 的 关系 。 

如 图 2. 9 所 示 ,假定 x4-y4s-z4 坐 标 系 统 是 固定 不 动 的 坐标 系统 , 称 为 参考 坐标 系 或 定 坐 
标 系 ,用 |4| 表示 。xs-ys-zs 坐 标 系 统 与 运动 刚体 固 连 , 称 为 动 坐 标 系 ,用 | BI 表示。 显然 ， 
刚体 在 定 坐 标 系 下 的 位 置 和 姿态 可 以 通过 动 坐标 系 在 定 坐标 系 下 的 位 置 和 姿态 确定 ,一 般 
而 言 ,刚体 的 位 置 与 姿态 对 应 于 动 坐标 系 原 点 及 各 坐标 轴 在 定 坐标 系 的 位 置 与 方向 。 
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图 2.9 坐标 系 的 建立 


2.2.1 位 置 的 描述 


刚体 上 点 S 相对 于 定 坐标 系 的 位 置 可 以 通过 一 个 三 维 笛 卡 儿 和 天 量 忆 来 表示 ,如 图 2. 10 
所 示 , 如 果 点 S 各 坐标 分 量 表示 为 p, ,Pp, ,P;, 则 在 定 坐 标 系 141 下 ,矢量 p 表示 为 


t2. 1) 





其 中 ,p, ,p, pP: 是 点 S 在 坐标 系 141}1 下 沿 着 x,y, 轴 的 三 个 坐标 分 量 。zP 左上 角 的 4 表示 矢 
Ep EMAI 下 表示 ,矢量 也 可 以 写 为 

"p -p,i*p, j*p.k (2.2) 
AF, Jk 表示 沿 着 坐标 系 |14| 的 x,y 和 z 轴 的 单位 方向 。 在 坐标 系 14} 下 ,可 以 表示 为 


] 0 0 
l- 0 ,K-— 
0 0 l 


将 式 (2.3) 代 入 式 (2.2) ,显然 与 式 (2. 1) 相 同 。 如 果 矢 量 p 在 男 外 一 个 坐标 系 下 表示 ， 
即使 S 点 实际 位 置 不 变 , 其 沿 着 新 坐标 系 的 三 个 分 量 值 也 是 要 改变 的 ,因此 ,要 特别 注意 空 


T c (2.3) 
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间 点 所 在 的 坐标 系 。 

2.2.2 姿态 的 描述 

空间 一 个 刚体 ,不 仅 要 表示 其 在 空间 的 位 置 ,而 且 要 表示 出 在 空间 的 姿态 (Orienta- 
tion) 。 刚 体 的 姿态 ,可 以 由 固 连 于 此 刚体 的 坐标 系 |1B} 相对 于 定 坐 标 系 |41 的 姿态 来 表示 ， 
表示 方式 有 以 下 不 同形 式 :了 用 方向 余弦 表示 ;@ 用 欧 拉 角 或 其 他 的 转动 角 表 示 。 每 种 姿态 
的 表示 方法 都 有 自身 的 局 限 性 ,在 机 器 人 运动 描述 或 者 运动 控制 中 ,为 了 在 某 些 位 置 克服 当 
前 姿态 描述 的 局 限 性 ,经 常 需 要 在 不 同 的 姿态 描述 之 间 转 换 。 

1. 方 向 余弦 表示 


为 了 描述 动 坐标 系 181 相对 于 定 坐 标 系 141 的 姿态 , 先 假定 两 坐标 系 原点 重合 ,如 图 
2. 11 所 示 。 





图 2.11 姿态 的 描述 
AX, pss 分 别 表 示 沿 着 坐标 系 1B1 坐标 轴 的 三 个 单位 主 矢量 ,三 个 矢量 在 坐标 系 
|41 下 可 以 表示 为 xs sy, za, 写成 列 矩 阵 形式 分 别 为 




















AT T» l'i3 

A^ a^ A^ 

€X57|721|5.J57| | 47] "23 (2.4) 
T4 T4) T4; 


RP, [ra ra ra] 表示 坐标 系 |B| 的 xs 单位 矢量 在 坐标 系 141| 三 个 主轴 上 的 投影 ,其 他 
表示 类 似 。 把 上 面 三 个 方向 组 合 到 一 起 ,在 坐标 系 |41 下 ,坐标 系 1B| 的 姿态 显然 可 以 用 一 
个 3x3 的 矩阵 来 表示 , 即 

AR= [^x; ^y tza] - (2.3) 


la T5 FT 








Ey å Tu T3 
式 中 ,sR 为 坐标 系 | BI 相对 于 坐标 系 |141 的 姿态 矩阵 或 者 旋转 矩阵 ,上 标 4 表示 定 坐 标 系 标 
AE. FER B 表示 动 坐标 系 标签 ,4R 矩阵 中 三 个 列 矢量 xs ,ys ,zs 都 是 单位 矢量 , 且 互 相 垂 
直 , 并 满足 下 面 六 个 正 交 约束 条 件 : 

^x, . ^x, y, x ^y, = Zp p 4z =] 
4 E AT A^ A E (2. 6) 
Xp * yp= Jp Zp= Xg* Zp=0 


和 矩阵 sR 中 各 元 素 AA DOR SER 48927 RISE, BI 


^ 
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^ ^ 


X € 35 * X, Zg "Xa 
Ey €», Ys * Ja Z» * Ja 
X, * Z, $5 Zp * Z4 

JE, AS AR LATTEDSET Ae AR B) 姿态 矩阵 应 如 何 表 示 呢 ?按照 式 (2.7) 的 表示 方 
式 , 可 以 把 4R 写成 矩阵 形式 为 


AR-[^x, ^y, “zs] = (2.7) 








Xa *Xp 4 "Xp Xa" Xp 


:R=| Xa Ja Za] E "Jag Ja Fo iy 





(2.8) 
i. "Zp y», Zp Za * Zp 
比较 式 (2.7) 与 式 (2. 8) ,显然 得 
apR=(4R) 
利用 正 交 约束 条 件 , 参 见 式 (2.6) ,进一步 可 以 推 得 
(4R) R= 
(2.9) 


-D eR 

式 中 ,7 表示 3x3 AERE, 0T E AE E EA FERE EE 

用 方向 余弦 表示 旋转 矩阵 ,矩阵 包含 有 九 个 元 素 。 这 九 个 元 素 相互 依赖 ,满足 式 (2. 6) 
的 六 个 约束 条 件 ,因而 仅 需 要 三 个 独立 参数 就 可 以 描述 姿态 矩阵 。 但 也 面临 一 个 问题 , 即 难 
以 从 方向 余 苞 旋转 矩阵 中 直观 地 选择 出 三 个 独立 参数 来 表示 刚体 的 空间 砍 态 ,这 是 方向 余 
弦 表 示 方 法 的 缺陷 。 

【 例 2. 1】 BEHERA |B) 开始 与 定 坐 标 系 |4| 重合 , 动 坐标 系 |B} 绕 * 轴 旋 转 了 09 
角 后 ,坐标 系 |B| 在 定 坐 标 |4| 的 姿态 如 图 2. 12 所 示 , 用 方向 余弦 方法 表示 坐标 系 | B| 相对 
FEIRA |A) 的 旋转 矩阵 。 





X 
X47B 


图 2. 12 绕 % 轴 转动 时 两 坐标 系 姿态 
参照 图 2. 12 ,利用 式 (2.7) ,坐标 系 |B| 主轴 在 坐标 系 141 下 的 方向 为 

















1 0 0 
^x, 0 Ay, cos 0 ,4Z = —sin 0 
0 sin 0 cos 0 


SR 可 以 表示 为 


1 0 0 
sR=|0 cosð -sinb 


0 sing cos6 
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2.3  gfEAN [e] S Es 08 TIN BJ RACER 


2.3.1 平移 映射 

设 坐 标 系 181 与 14} 具有 相同 的 方向 ,但 两 者 之 间 原 点 不 重合 ,如 图 2. 13 所 示 。 图 中 ， 
^P sonc KINEMA | B] 的 原点 在 坐标 系 14} 下 的 位 置 矢量 。 如 果 点 $ 在 坐标 系 |B) 下 的 位 置 
矢量 为 *p ,那么 它 在 坐标 系 |41 下 的 位 置 矢量 定义 为 p, 可 由 矢量 相 加 得 到 , 即 

We" Punot p (2. 10) 

2.3.2 jetet} 

假定 坐标 系 1B8 S514 原点 重合 ,但 方向 不 同 , 如 图 2. 14 所 示 , 两 坐标 系 之 间 的 旋转 矩 
阵 为 52R。 已 经 知道 在 坐标 系 |B} F, SARUTE EA pp, "p, p), REBRA IA) 
下 5 点 的 位 置 矢量 Po 


Zg 





B; 4j 





X4 


图 2.13 点 位 置 矢 量 平移 映射 图 2. 14 点 位 置 矢量 旋转 映射 
在 坐标 系 141} 下 ,矢量 ' 己 可 以 表示 为 
^p -^ p," x 4" p, y P: Ze (2. 11) 
改写 成 矩阵 的 形式 为 
"P. 
"ps [es ^y Azp] P =p Pp (2. 12) 


B 


p. 

XX (2. 12) 表 明 两 坐标 原点 重合 时 ,空间 同一 个 点 在 坐标 系 | Bi 下 的 位 置 撩 量 "p 映射 到 
MERRIA! Rp 表达 方式 。 此 时 在 不 同 的 坐标 系 下 ,点 的 矢量 方向 发 生 改 变 , 模 长 不 变 。 通 
第 ,坐标 系 |B| 原 点 与 坐标 系 |4| 既 不 重合 ,方向 也 不 相同 ,如 图 2. 15 所 示 , Peor EIR EER 
系 |1B| 原 点 在 坐标 系 |4|} 下 的 位 置 矢量 ,sR 表示 两 坐标 系 的 旋转 和 矩阵 ,*p DK p. BRE 
存在 一 个 临时 坐标 系 |S1 ,该 坐标 系 与 坐标 系 |41 的 方向 一 致 ,原点 与 坐标 系 |B| 重合 。 
坐标 系 1S} 下 ,点 的 位 置 矢量 p 可 以 表示 为 

P=sR'p 
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图 2. 15 一 般 的 点 位 置 矢量 映射 
点 的 位 置 天 量 在 坐标 系 13|} IAS BR AR LA] 的 映射 为 


"pe 
P= P+ Psonc 


合并 以 上 两 式 为 
"p - SR p^ Pror (2.13) 
X (2. 13) 改 写成 齐 次 矩阵 的 形式 为 
ze v a 
简写 成 
WE eas 


式 中 ,s7eR ”矩阵 表示 坐标 系 181 相对 于 坐标 系 141 齐 次 变换 矩阵 ,包含 两 坐标 位 置 与 次 
态 的 关系 。 


2.3.3 复合 映射 


如 图 2. 16 所 示 , 图 中 有 |14} , | BERI C] 三 个 坐标 系 , 各 坐标 系 齐 次 变换 矩阵 47 ,47 已 
Al, TEASERAR IC] 下 ,已 知 点 $ 的 位 置 矢量 “p ,如 何 求 得 p。 已 经 知道 的 关系 为 


BT- ja TN AT- n vu 
0 ] 0 l 
点 $ 在 坐标 系 1C} 下 的 矢量 为 也, 映射 到 坐标 1B8} 下 的 矢量 多 为 


-人 

点 5 在 坐标 系 |1B| 下 的 矢量 为 p ,映射 到 坐标 系 |4| 下 的 矢量 和 p 为 
HEH 
HE 


EPRA | C) 相对 坐标 系 |4| 的 齐 次 变换 矩阵 为 


合并 以 上 两 式 , 得 
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sR cR BR "P conc * P - 
l 
在 坐标 系 |1C| 下 点 的 位 置 撩 量 , 左 乘 (T 即 可 得 出 此 点 在 坐标 系 14} 下 的 位 置 矢量 ,对 于 
多 坐标 系 之 间 的 映射 ,也 可 以 参照 上 述 方法 ,得 出 点 在 不 同 坐 标 系 下 的 映射 ,并 得 出 各 坐标 
系 之 间 的 齐 次 变换 矩阵 。 


cT-5T;T - 





图 2.16 点 的 复合 映射 


2.3.4 齐 次 抵 阵 的 逆 


坐标 系 |B| 相对 于 坐标 系 141 的 齐 次 矩阵 描述 为 se7F ,但 是 在 机 器 人 运动 学 \ 动 力学 的 计 
算 中 ,为 了 简化 计算 ,存在 空间 点 的 位 置 天 量 在 不 同 坐标 系 下 的 相互 转换 问题 。 例 如 ,已 知 
点 在 坐标 系 1B+ 下 位 置 天 量 ,当前 计算 需要 变换 到 坐标 系 14} 下 ,计算 完成 后 又 需要 变换 到 
IRR IB) 下 ,这 里 就 需要 解决 坐标 系 14} 相对 于 坐标 系 |1B| 的 描述 47, 此 问题 是 齐 次 矩阵 
求 逆 的 问题 ,可 以 按照 以 前 所 学 的 数学 知识 ,对 4x4 齐 次 变换 矩阵 求 逆 ,也 可 以 利用 两 坐标 
系 齐 次 变换 矩阵 特殊 性 质 求 道 。 下 面 讨 论 齐 次 变换 矩阵 的 求 逆 方法 。 


sR “Poore 
BAIRR |B| 相对 于 坐标 系 141 的 齐 次 变换 矩阵 为 7= [” “| sper - 
AR 
la To APRIRE. 
首先 根据 已 知 的 sR 计算 4R ,根据 式 (2.9) ,得 
AR=(sR)" 

f| FH P sonc TT 53. Paonc ,如 图 2.17 所 示 ,并 根据 式 (2. 13 ) 得 

"U Pane) = ik Poo t Pase (2. 16) 
显然 ,矢量 "Psone 在 坐标 系 141 下 所 指向 的 点 是 坐标 系 1B8} 的 原点 ,在 坐标 系 |1B| 下 是 零 


Ad BA XO. 16) 左 侧 为 零 撩 量 , 可 以 进一步 写 为 
0 R put Pino 
即 
"e = -4 R’ P sorc = 一 人 BR) y 
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最 后 得 出 47 为 
(5R)' —(sR) Pase 
0 l 


T= (2.17) 





图 2.17 JFÜOBPESRGSUR XE ESI 


2.4 齐 次 坐标 变换 


映射 表示 空间 同一 点 在 各 坐标 系 相 对 关系 已 知 的 前 提 下 ,在 不 同 的 坐标 系 下 的 矢量 描 
述 。 而 齐 次 坐标 变换 更 多 体现 了 刚体 在 空间 的 运动 ,刚体 的 一 般 有 限 运动 是 指 刚体 从 一 个 
位 姿 运动 到 夯 一 个 位 姿 ,运动 过 程 伴随 着 移动 和 转动 。 


2.4.1 平移 变换 


刚体 在 空间 做 平移 运动 ,点 S 为 刚体 上 的 一 个 点 ,如 图 2. 18 所 示 ,刚体 在 空间 沿 着 在 参 
FEMRA 下 的 矢量 q 方向 做 平移 移动 ,点 5 移动 到 点 $' ,开始 时 刚体 上 点 S 在 坐标 系 
|A) 下 的 位 置 矢量 为 已 ,运动 后 点 S' 在 坐标 系 |4| 下 的 位 置 矢量 为 p', 此 时 坐标 系 |4} 下 的 
S$' 扩 的 位 置 拓 量 p' 可 以 表示 为 

p'-q*p (2.18) 
xX'P.q-[q, aq, 4.] 表示 矢量 4 在 坐标 系 |4} 下 沿 各 轴 的 坐标 分 量 。 如 果 矢 量 p',p 用 
齐 次 坐标 表示 , 式 (2. 18) 可 以 改写 为 


p' - Trans(q, ,0,,4.)P (2 19) 
1 UÜ D 4 
0 l 0 dy —— T NS RE 
INP , Trans(4,,4,,4.) 7 M Wa 表示 平移 变换 或 平移 运动 运算 。 式 (2. 18) 和 (2. 19) 
q: 
O uU D ł 


所 有 矢量 均 在 坐标 系 |4} 下 表示 。 
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XA 


图 2.18 和 平移 变换 


2.4.2 旋转 变换 

如 图 2. 19 所 示 ,刚体 在 空间 做 定点 旋转 运动 ,点 S 为 刚体 上 的 一 点 ,初始 在 参考 坐标 系 

(AL FEREN p, RERA A) 原点 的 轴线 工 旋 转 了 6 角 , 轴 线 的 空间 单位 方向 为 e, 此 
时 点 S 运动 到 点 $ YEAR AR (LAT 下 的 矢量 为 p'。 对 于 刚体 空间 的 旋转 运动 ,可 以 表示 为 

p' - Rot(e,0)p (2. 20) 


A Rot 表示 旋转 变换 运算 ,Rot(e,0) 表 示 在 坐标 系 |4| 下 绕 过 原点 单位 方向 为 e 的 轴线 
旋转 9 角 。 此 运算 可 以 写 为 齐 次 变换 的 形式 ,其 位 置 矢量 部 分 为 零 。 





图 2. 19 ”组 空间 轴线 的 旋转 变换 
TEASER AR LA] 下 ,最 基本 的 旋转 是 绕 坐 标 轴 zx 轴 、y 轴 和 = 轴 的 旋转 ,为 了 分 析 方 便 , 如 
图 2. 12 所 示 ,假定 建立 一 个 坐标 系 {1B81 与 刚体 (未 画 出 ) 固 连 , 绕 坐标 系 14} 的 x 轴 旋 转 0 
ff ,刚体 的 旋转 运动 显然 可 以 用 两 坐标 系 的 瞬时 姿态 关系 表示 ,旋转 后 利用 式 (2.7) 可 以 得 


出 














] 0 0 
xe: 0 yas c0 m -s0 
0 SO cO 








刚体 绕 x 轴 旋 转运 动 的 基本 旋转 齐 次 变换 矩阵 可 以 表示 为 
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IOU Q Q 
^ 0 c0 -s0 0 
a 0 s0 c0 0 
0. 0 Q 1 
同样 , 绕 y SHURII z 轴 的 旋转 齐 次 变换 矩阵 为 
c0 0 s0 O 
" 0 I1 O 90 
Reyes nod. d 
0 0 0 1] 
和 
c -s09 0 0 
^ 30 ,80.-0 0 
Rot(z,0) - & 5 1 [2 441) 
0 .-0- O0 1 


式 中 ,cg=cos 0,50 sin 9,x 表示 x 轴 的 单位 方向 ,其 他 类 似 。 

1. 绕 沿 定 坐 标 系 运动 

已 知 两 坐标 系 14} , 18 开始 时 重合 ,141 为 定 坐 标 系 ,1B8} 固 连 于 刚体 (未 画 出 ) 与 刚体 
一 起 运动 ,刚体 上 一 点 为 $ 点 ,矢量 为 &=[6 4 3] ,如 图 2.20 所 示 。 坐 标 系 1B} 开始 绕 z 
轴 转 动 90? ,转动 后 所 形成 的 临时 坐标 系 为 {B11 ,点 5 运动 到 点 $ ,在 坐标 系 1B 下 ,矢量 zw 
不 变 , 在 坐标 系 |41} 下 点 S' 的 位 置 矢量 表示 为 v, 如 图 2. 21 所 示 , 依 据 式 (2. 15 ) 计 算 为 

0 -1 O OIl[6 一 人 
^y 5 T^i - Rot(z,90?) "y 3 


0 





图 2.20 初始 位 姿 图 2.21 绕 z 轴 旋转 90° 
ARAS p I GB, j 继续 绕 坐 标 系 |4| 的 y 轴 方向 旋转 90°, 形 成 临时 坐标 系 |B,| ,点 S' 运 
动 到 点 S" ,在 坐标 系 141 下 点 5S" 的 位 置 矢 量 表示 为 w, 这 一 变换 可 以 从 图 2. 22 中 看 出 ,表示 
为 


A _A pb 
w=}, T °u 


显然 ,在 坐标 系 |B,| 下 ,矢量 w 不 变 。 上 式 中 4 了 可 以 进一步 表示 为 
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5, T - Rot( y,90?) ; T 

以 上 公式 可 以 解释 为 :pT 表示 坐标 系 |Bi | 相对 于 坐标 系 |4| 的 齐 次 变换 矩阵 ,第 一 次 
旋转 后 ,坐标 系 |B, | 相对 于 坐标 系 141 的 齐 次 变换 和 矩阵? T 中 各 列 和 矩阵 分 别 表示 坐标 系 
(B LIS x,y FI z 坐标 轴 在 坐标 系 |41 下 的 方向 和 原点 的 位 置 ,第 二 次 旋转 , 即 左 乘 Rot(y， 
90°) 表 示 把 此 时 坐标 系 |B1 1 在 坐标 系 |4) 下 的 方向 和 位 置 再 次 变换 到 坐标 系 |141 下 ,形成 
临时 的 坐标 系 |B, | 在 坐标 系 |141 下 的 方向 和 位 置 。 如 果 把 上 面 两 次 旋转 变换 组 合 在 一 起 ， 
那么 可 以 得 出 

Aw=Rot(y,90°)Rot(z,90°)u=[3 6 4 1]' 

因为 矩阵 的 乘法 不 具有 交换 性 ,所 以 对 于 刚体 绕 定 坐标 系 的 转动 ,其 变换 矩阵 要 左 乘 ， 
即 变换 顺序 为 从 右 向 左 。 上 述 两 次 旋转 变换 后 ,点 $ "在 定 坐 标 系 下 平移 [5 -2 6] ,最 终 
得 到 点 S” 在 定 坐 标 系 下 的 位 置 为 

ps,=Trans(5,-2,6)Rot(y,90°)Rot(z,90°)u=[8 4 10 1]' 
如 图 2.23 所 示 , 有 兴趣 的 读者 可 以 自己 进行 计算 ,检验 平移 变换 是 否 可 以 随意 左 乘 与 





图 2.22 BEERA y 轴 旋 转 90° 图 2.23 沿 定 坐 标 系 坐标 轴 移 动 
2. 绕 沿 动 坐标 系 运动 


刚体 旋转 运动 ,不 是 绕 定 坐标 系 各 轴 转 动 , 而 是 绕 与 自身 固 连 的 坐标 系 转动 ,其 变换 顺 
序 又 是 如 何 呢 ? 已 知 两 坐标 系 开始 时 重合 ,在 坐标 系 1BI 下 点 5 的 矢量 为 u = 
[6 4 3 1] ,如 图 2.20 所 示 , 点 5 随 坐 标 系 |B| 一 起 运动 ,首先 坐标 系 |1B} 绕 z liia 
90°, 临 时 形成 的 坐标 系 用 | B11| 表示 ,在 坐标 系 |141 下 ,点 的 5S 运动 到 点 $' ,如 图 2. 21 所 示 ， 
Ry BUT Y 为 


ü ei d oig re 
: i 0 O0 óoll4l | $6 

"yug T u-Ret(z,909)u— = 
Bı ui Pom T8814 
0 0 0 1 1 


而 后 ,坐标 系 1B | 绕 其 自身 y 轴 旋 转 90? ,得 到 的 临时 坐标 系 用 1 B,| 表示 ,如 图 2. 24 所 
示 , 点 $ 运 动 到 点 $ ,点 8 在 坐标 系 1B 下 的 位 置 矢量 "w 可 以 表示 为 
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0 0 1 01r6 3 

Bi y =21 TP2u =Rot( y ,90°)u = e ONNA Ab 
B2 , vl 0 0 Ol -6 

ô BH D LILI 1 


点 8 的 位 置 矢量 w 从 坐标 系 | B,} 变换 到 坐标 系 141 下 为 
^w T^ w - Rot(z,90?) "w - Rot(,90?) Rot(y,90?)u 
所 以 对 于 刚体 绕 自身 坐标 系 (或 动 坐标 系 ) 的 转动 ,其 变换 矩阵 要 右 乘 , 即 变换 顺序 为 
从 左 向 右 。 





图 2.24 绕 动 坐标 系 y 轴 旋 转 


【 例 2.2】 坐标 系 |B} 初 始 与 坐标 系 |4| 重合 ,而 后 坐标 系 |B| 做 平移 运动 ,移动 到 点 
[5 4 1] ,然后 绕 坐标 系 |B| 的 x 轴 旋 转 了 30°, 最 后 绕 通过 点 [2 0 2]" 上 且 方 向 与 坐标 
ALBIS y 轴 平 行 的 直线 旋转 了 60°, 经 过 上 述 运 动 后 , 求 坐标 系 |B} 相对 坐标 系 141 的 齐 次 
变换 矩阵 87。 首 先 坐标 系 181 沿 坐标 系 141 的 坐标 平移 后 ,临时 坐标 系 记 为 1C} ,坐标 系 
(CI 51A] 之 间 的 齐 次 变换 矩阵 为 


1 0 9 5 
jä 0 1 0 4 
cT - 

D Jg I I 

B Un. 


坐标 系 1B} 继续 运动 , 绕 其 自身 x 轴 旋 转 30° 后 ,临时 坐标 系 记 为 1D} AS ERI IDI S 
1C} 之 间 的 齐 次 变换 矩阵 为 
1 0 0 0 
, 0 cos30? -sin 30° 0 
P” |O sin30° cos30。 0 
0 0 0 ] 
为 了 绕 在 自身 坐标 系 下 ,过 点 [2 0 2] 且 与 y 轴 平 行 的 轴线 旋转 60°, 坐 标 系 |B| 先 
平移 到 点 [2 0 2] ,临时 坐标 系 记 为 | 五 | ,坐标 系 1 歼 与 1D 之 间 的 齐 次 换 矩 阵 为 


i UU oU 3 

U I. 0 0 
"T- 

0 0 1 2 

Ü 0 0 1 
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然后 绕 y 轴 旋 转 60? ,旋转 完成 后 ,临时 坐标 系 记 为 1 了 | ,坐标 系 | 下 与 和 181 之 间 的 齐 次 
变换 矩阵 为 


cos 60? 0 sin60? 0 
r 0 0 0 0 
rT = , 
—sin 60? 0 cos 60? 0 
0 0 0 ] 
最 后 ,平移 [-2 0 -2] ,此 时 坐标 系 |B| 与 坐标 系 |F| 的 齐 次 变换 矩阵 为 
1 0 0 -2 
U I BH Q 
8 了 = 
0.0 1 -2 
00g 1 


47 最 终 得 到 
Hy Ae pl get ipt ar a 


2.5 刚体 的 旋转 


2.5.1 欧 拉 角 表 示 法 


刚体 在 三 维 笛 卡 儿 空 间 的 运动 包含 平移 与 旋转 运动 ,平移 运动 可 使 用 三 个 笛 卡 儿 坐 标 
系 定 义 , 而 转动 需要 三 个 旋转 角度 坐标 定义 ,因此 刚体 的 运动 可 使 用 六 个 坐标 完整 地 确定 。 
在 稍 卡 儿 坐 标 系 下 ,可 以 通过 连续 三 次 且 相 邻 两 次 旋转 不 可 以 绕 相 同 的 坐标 轴 的 旋转 运动 ， 
来 描述 转动 刚体 相对 于 参考 坐标 系 的 方向 ,三 个 转动 的 角度 参数 可 作为 广义 坐标 ,也 称 欧 拉 
角 。 当 给 定 定 坐标 系 和 动 坐 标 系 后 ,可 以 有 24 种 不 同 的 旋转 次 序 描述 刚体 相对 于 定 坐 标 系 
的 姿态 。 下 面 介 绍 两 种 最 常用 的 欧 拉 角 。 

1. z-y- 欧 拉 角 

定 \ 动 坐标 系 自重 合 位 姿 开 始 , 动 坐标 系 首先 绕 z 轴 转 动 a 角 ,接着 绕 自身 y 轴 转 动 B 
角 , 最 后 表 绕 其 自 喘 z fite a y 角 , 其 旋转 过 程 如 图 2.25 所 示 ,旋转 后 ,坐标 系 {1B} 相对 于 
Ak zs LA] 的 方向 矩阵 为 

4R =Rot(z,a)Rot(y,B)Rot(z,y)= 











ca -sa Ol æ 0 sg|[cy -sy 0 
sa ca OjO 1 Illy cy O (2:22) 
0 0 1jL-£ 0 æILO 0 1 


式 中 ,sR.- 称 为 z-y-z 欧 拉 角 变 换 。 以 上 矩阵 相 乘 ,得 
cacBcy-—sosy | —cacBsy—sacy  casB 
sacßcy+casy -sacBsytcacy sasB (4. 23.) 


-sBcy sBsy cp 


4 
BR az * 
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BR=| ry ry T3 (2. 24) 





x 
B, 


图 2.25 zyz Kx ht ff 
为 了 求 得 zy- 欧 拉 角 变 换 的 三 个 欧 拉 角 , 令 
BR.: -;R 
首先 ,假定 ri ,rm 不 全 为 零 ,那么 可 以 知道 80,80 或 B 关 180°, 即 ,rs 不 全 为 零 ， 
r3 天 十 。 那 么 三 个 欧 拉 角 可 以 表示 为 
B= DA 和, 
a — atan2 ( r,/sB ,ra3aVsB ) (2. 25) 
y 2 atan2 ( r3/ sB , —r4,/ s) 
虽然 B 角 存 在 第 二 个 解 , 但 在 式 (2. 25) 中 计算 得 到 的 B 角 的 值 域 为 0<8<180", 绕 z 轴 、 
y 轴 和 z 轴 依 次 转动 计算 所 得 的 a,B I 角度 ,可 得 出 给 定 的 旋转 矩阵 sR 所 表示 的 方向 。 
如 有 果 B=0 或 B=180°, 意 味 着 73=rs=0,r=73=0, 此 时 的 解 出 现 退 化 ,和 矩阵 sR 有 以 下 的 形 


A: 
ri rj 0 
fa d O | 
0 O0 zl 
如 果 rs, 7 1, IET 8-0. 0,5 RB EAE A 
cacy-sosy  —casy-sacy 0 c(a*y) -s(a*y) 0 
^ | | 


R= 





sacy+casy -sasy+cacy 0 s(a*y) cla+y) 0 
0 0 l 0 0 1 
那么 w+y 得 
æ+y=atan 2(r,, ri ) 
如 果 rs =-1, 此 时 B=180",sR.- 矩 阵 变 为 
-c(a-y) -s(a-y) 0 
-| s(a—-y)  c(a-y) 1 
0 0 l 
那么 a-y 得 
Q—y=atan2( -r,, ,一 ri ) 
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如 果 出 现 解 退 化 的 情况 ,习惯 令 w=0 ,那么 上 述 两 种 特殊 情况 所 对 应 的 解 如 下 。 
如 果 B=0 ,一 种 解 的 情况 为 

B=0 

a=0 

y=atan 2( -7,5 ,Tril ) 
如 果 B=180.0" ,一 种 解 的 情况 为 

B=180° 

&=0 

y -atan 2( ras Ta) 


(2. 20) 


(2.27) 


2. RPY 方式 


另 一 种 表示 转动 的 欧 拉 角 为 滚动 (Roll) 俯仰 (Piteh ) 和 偏转 (Yaw) 角 (简称 RPY ff), 
这 种 转动 方式 主要 用 于 航空 航海 .机 帮 人 末端 姿态 描述 等 运动 分 析 中 ,如 图 2.26 所 示 。 它 


的 转动 次 序 依次 绕 定 坐 标 系 的 * 轴 转动 角 , 绕 y 轴 转 动 09 角 , 绕 z 轴 转 动 p 角 , 三 次 旋转 
变换 计算 如 下 : 





图 2.26 RPY 欧 拉 角 


AR,,=Rot(z,p)Rot(y,0)Rot(x,y)= 


cp -sp O| Æ 0 sæ|[1 O 0 
: cQ 1 | 0 1 j 0 cy a = 
0 O IJL-s O c801LO sy cy 
- cQsOsy-spojb copsOcy *spsy 





spcO — sqsOsiy*cpoj spsOcy -cosp 
一 SO cOsy cOcy 
给 定 sR, 见 式 (2. 24) ,如 果 % 关 0 ,三 个 欧 拉 角 可 以 解 为 


0=atan 2( -rs m ET 
p=atan 2(r4,/c0,r,,/c0) (2.29) 
ij =atan 2(r4,/c0 ,r4,/c0) 
在 式 (2. 29) 中 ,尽管 9 存在 第 二 个 解 ,但 通常 计算 所 得 的 9 值 域 为 [ 7909,90? ] ,这 样 可 
以 保证 姿态 的 表示 和 三 个 欧 拉 角 一 一 对 应 ,但 同时 也 要 注意 在 某 些 情况 下 ,给 定 旋转 矩阵 计 
算出 所 有 人 解 也 是 必要 的 。 当 9=+90°, 式 (2. 29) 的 解 退化 ,此 时 可 以 按照 惯例 定义 ,通常 令 
p=0, 则 当前 解 表示 如 下 : 
如 果 9=90° ,一 种 解 的 情况 为 


(2, 28) 
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02:90? 
p=0 (2. 30) 
iy —atan 2(rj; ,r;;) 
如 果 9=-90°, 一 种 解 的 情况 为 
0=-90° 
p=0 (A: 31) 
ij — —atan 2 (rT; ,rz ) 

通过 对 以 上 两 种 欧 拉 角 的 分 析 得 知 ,对 于 绕 上 自身 旋转 的 zyz 角 , 当 B=0 或 B=180°, 解 出 
现 退 化 ,对 于 绕 定 坐标 系 旋转 的 RPY 角 , 当 9=+90° 时 , 解 出 现 退 化 。zyz 角 的 优点 在 于 几何 
意义 直观 明确 ,对 于 一 个 给 定 的 zyz 角 , 可 以 迅速 将 刚体 的 姿态 想象 出 来 ,如 图 2. 27 所 示 。 
RPY 角 的 优点 主要 有 两 个 :一 是 由 于 存在 各 种 约束 ,机 器 人 工作 空间 内 各 RPY 角 的 变化 范 
围 一 般 都 不 会 超过 -90° ~90°, 具 有 了 唯一 性 ;而 zyz 角 中 的 a 角 的 变化 范围 为 -ww ~ +o ,因而 
用 zyz 角 表 达 并 不 唯一 ;二 是 zyz 角 的 退化 点 为 B=0°, 这 个 点 一 般 位 于 正常 的 工作 空间 内 ， 
而 RPY 角 虽 然 在 6=+90° 时 也 产生 了 退化 ,但 该 点 经 常 位 于 工作 空间 之 外 , 故 一 般 不 会 对 分 
析 计 算 产 生 影响 。 因 此 ,在 实际 应 用 中 应 根据 场合 的 需要 决定 采用 zyz 角 还 是 RPY 角 。 

绕 目 喘 坐 标 系 旋转 的 欧 拉 角 如 果 选 择 合理 ,可 以 和 机 器 人 关节 实际 转动 符合 ,在 分 析 机 
构 运 动 时 具有 较 强 的 直观 性 ,这 是 绕 定 系 旋转 的 欧 拉 角 无 法 比拟 的 ,如 图 2. 28 所 示 的 空间 
3R 开 链 手臂 ,关节 转动 轴 相 交 于 一 点 ,如 果 在 第 1 关节 轴线 与 基 座 交点 处 建立 定 坐 标 系 , 在 
机 械 手 末端 建立 工具 坐标 系 ,那么 三 个 关节 的 运动 必然 造成 工具 坐标 系 方 向 相对 于 定 坐 标 
系 的 变化 ,为 了 描述 这 种 变化 ,在 关节 轴 交 点 处 建立 一 个 临时 坐标 系 |B) ,坐标 轴 与 关节 轴 
轴线 重合 ,开始 时 此 坐标 系 与 定 坐 标 系 方向 重合 ,如 果 选 择 绕 此 坐标 系 z-y-x 轴 转 动 来 描述 
工具 坐标 系 的 姿态 ,显然 三 个 欧 拉 角 与 三 个 关节 的 实际 转动 角度 相符 。 如 果 采 用 绕 定 坐标 
系 的 x-y-z 轴 也 可 以 得 到 相同 的 结果 ,但 缺乏 直观 性 。 





图 2.27 zyz 角 示 意图 图 2. 28 三 关节 手臂 绕 第 2 关节 转动 
2.5.2. 一 般 的 旋转 变换 


前 面 已 经 研究 了 绕 轴 x 轴 \y 轴 或 z 轴 的 基本 旋转 变换 矩阵 ,根据 欧 拉 定理 :刚体 绕 定点 
的 任意 有 限 转 动 可 由 绕 过 该 点 某 轴 的 一 次 有 限 转动 实现 ,现在 研究 最 一 般 的 旋转 变换 情况 ， 
即 旋 转轴 不 与 任何 参考 系 的 轴线 重合 ,而 是 绕 参考 系 中 从 原点 出 发 的 某 一 矢量 旋转 一 个 角 
度 时 的 旋转 矩阵 。 如 图 2. 19 所 示 , 考 虑 空间 点 S 绕 直 线 世 旋转 角度 9 得 到 S' , 直线 L 的 单 
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位 矢量 为 e=[e。e，e:] 。 
从 图 2. 19 可 以 明显 看 出 
p'=00+05' 
式 中 ,00 是 p WREE e 的 轴 向 平行 分 量 ,表示 为 
00=(p .e)e=ee'P 
从 图 2. 19 可 以 看 出 


QS' - ( cos 0) QS4.( sin 0) QS" 


其 中 ,@3 是 p 的 与 e 垂直 的 分 量 , 即 
QS- (1-ee")p 
Jt ROSA 
QS" - exp - Ep 
式 中 ,E ERE e Tg th BERDSEERAB B , BI 





将 式 (2. 35) 和 式 (2. 36) 代 人 式 (2.34) ,得 


nc T 
QS' =cos 0(I-ee )p-sin 0Ep 


将 式 (2. 33) 和 式 (2. 37) 代 入 式 (2. 32) ,得 


p'-ee p«cos 0(T-ee )p-sin OEp 
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(2. 32) 


(2. 33) 


(2. 34) 


(2.39) 


(2. 36) 


(2.37) 


(2.38) 


式 (2. 38) WH , p' Ji p 的 垂直 分 量 绕 e 旋转 0 角 后 与 p 的 轴 问 平行 分 量 的 合成 , 式 


(2. 38 ) 价 化 为 


p' - [ ee! +cos 0( I-ee! )+sin 8E ]p 
式 中 [ee' +cos 9(1-ee ) «sin 8E |] 和 矩阵 ,就 是 所 求 的 旋转 变换 矩阵 , 即 
R - Rot( e,0) - ee! +cos 0( I-ee! )+sin 0E 


在 特殊 情况 下 ,24 02m 时 
R--I-«2ee' 


(2. 39) 


(2. 40) 


显然 , 当 9="7 时 ,R ERRER; 0-0 时 ,R=1。 前 面 所 研究 的 绕 坐 标 系 各 轴 的 基本 


旋转 变换 ,可 利用 上 面 推 导 得 出 。 例 如 , 绕 x 轴 的 旋转 变换 矩阵 为 

















1 0 0 0 1 
e-|0|,E-|O 0 -ll|,ee = 
0 0 1 0 
代入 式 (2. 40) , € x 轴 旋 转 为 
ItO 0 
R:(0)=|0 c0 -s0 
0 s0 cb 








同 理 ,可 以 求 得 绕 过 原点 方向 为 e 的 直线 了 工 ,旋转 9 角 的 旋转 矩阵 为 


0 0 0 
B ug 
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e,e,v0+c0 X e,e,v0—e,sÓ ee:00A+e,30 
R,(0)-|e,e,v0*e,s0 | e,e,vO*cO — e,e,v0—e,sO (2.41) 
e,e,vU—e,sÜ e, e v0+e,s0 ^ e,e,vO*cO 
式 中 ,cg=cos 0,s0-sin 0,v0-1-cos 0。 转 动 角 0 的 符号 通过 右手 法 则 确定 ,拇指 指向 e 的 正 
[E] o 
如 果 给 定 一 个 旋转 矩阵 ( 式 (2. 24) ) ,可 以 求 得 等 效 的 转动 轴 和 转动 角 。 令 
R(0)- ;R 
上 式 两 边 的 对 角 线 项 分 别 相 加 ,并 简化 ,得 


Tii Hra +ras = 1+2c0 


进一步 ,得 
9=acos| amet) (2,42 ) 
非 对 角 线 项 成 对 相 减 ,得 
1732 一 723 
l 
e=} mm (2.43) 
T»? Tl, 








0 解 得 值 域 为 10 ”180°] ,上 述 解 写 可 以 写 为 (e,9) ,但 也 要 注意 在 空间 存在 另外 一 个 解 
(-e,-6) ,因此 从 旋转 矩阵 变化 成 等 效 的 转轴 、 转 角 表 示 面 临 解 选择 的 问题 。 同 时 ,小 角度 
转动 可 能 造成 解 的 病态 , 当 0-0 或 96=180°, 将 会 无 法 求解 。 


2.5.3 Euler-Rodrigues 参数 


另外 有 一 种 对 姿态 的 表示 方式 ,可 以 采用 四 个 元 素 ,构成 欧 拉 - 罗 德 里 格 斯 参数 (Euler- 
Rodrigues) 或 四 元 数 。 四 元 数 本 身 有 以 下 特点 :中 四 元 数 不 会 产生 欧 拉 角 表示 旋转 存在 的 在 
0° 或 90° 的 解 退化 问题 ; 忆 四 元 数 由 四 个 数组 成 ,旋转 矩阵 需要 九 个 数 ;@ 两 个 四 元 数 之 间 
更 容易 插值 ,在 姿态 插 补 时 更 平滑 ;@ 引 四 元 数 .矩阵 多 次 运算 后 产生 计算 累计 误差 ,需要 分 别 
对 其 做 规范 化 和 正 交 化 ,对 四 元 数 规范 化 更 容易 ;二 与 旋转 抢 阵 类 似 , 两 个 四 元 数 相 乘 表 示 
两 次 旋转 。 因 此 ,在 当今 工业 机 器 人 控制 器 内 机 器 人 运动 学 的 计算 大 多 采用 四 元 数 法 。 

如 果 已 知 等 效 的 轴 e 和 等 效 的 角 0, Euler-Rodrigues 参数 可 以 定义 为 
di 
d2 
d3 


qo =cos( 7 二 -sin( 5) e (2. 44) 








式 (2.44) 可 以 分 开 写 为 
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ES 


xr xr Sn 
CN 
ad 


(2.45) 


CN 
E 


~ 
Il 
un 
. = + 
=] 
Qi 0—— RS — 


njo njo njo n 


LA 
c 
LS 


Euler-Rodrigues 参数 用 qo 和 gq, 来 表示 旋转 ,这 是 一 种 利用 四 个 参数 来 表示 旋转 的 形式 ， 
因而 也 称 四 元 数 法 。Euler-Rodrigues 参数 如 何 表示 为 一 种 旋转 ,详细 过 程 请 参考 附录 4, 通 
过 其 推导 过 程 ,其 物理 意义 一 目 了 然 。 显 然 ,四 个 分 量 符合 以 下 约束 条 件 : 

qo+qg1+92+g3=1 (2. 46) 

根据 附录 A 式 (45) ,由 四 元 数 构 成 的 旋转 矩阵 为 

go0+91-92-g3  2(q4142-4443) 2(9193+gog2 ) 
R,=|2(qıq2+40943)  4o-4:*4;-43 2(9,93-qo0g1) 
2(q,44-494;:) 2(9293+gogi) — qo-4i—42*45 
如 有 果 给 定 旋转 和 矩阵 , 则 Euler-Rodringues 参数 可 以 计算 为 
q= 7 /lrj-r5*r4 
E (2.47) 
E de 

DES , UREE E Po Z6: fefe 180" , 则 go =0 ,无 法 利用 公式 (2. 47) 计算 Euler- 
Rodringues 参数 。 此 时 ,参数 可 以 利用 以 下 公式 计算 : 

y= 


] 

qi ni ] tr; 772774 
] 

92 =V 1 +r2zz 一 r33 一 ril 
1 

q3 79V ] *r4, 7r; 772; 


参考 附录 A 式 (51) ,注意 到 Euler-Rodringues 参数 [0 qi h de] 和 [0 "i “ga -gq3] 


表示 的 旋转 相同 ,所 以 公式 也 无 法 确定 此 时 四 元 数 矢量 部 分 参数 的 正 负 号 ,因此 需要 一 种 通 
和 的 处 理 方法 ,该 方法 参见 附录 Bo 


3 


(2.48) 
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本 章 小 结 


本 章 在 得 出 两 坐标 系 之 间 描 述 位 置 与 姿态 的 方式 后 ,用 齐 次 变换 和 窍 阵 描述 空间 相同 的 
点 在 不 同 坐 标 系 下 的 位 置 映射 , 可 以 知道 机 项 人 任务 空间 点 的 位 置 可 以 通过 映射 的 方法 ,得 
出 在 不 同 坐 标 系 下 的 位 置 天 量 。 本 章 采 用 齐 次 变换 矩阵 描述 刚体 的 运动 ,其 与 点 在 不 同 坐 
标 系 下 的 映射 有 相似 之 处 ,但 映射 更 多 地 体现 了 一 种 静态 的 关系 ,而 变换 用 来 分 析 刚 体 的 运 
动 ,体现 了 一 种 动态 的 关系 ,要 注意 两 者 之 间 的 细微 差异 。 本 章 的 内 容 为 其 后 运动 学 的 分 析 


提供 了 基本 的 理论 基础 。 


第 3 章 机 希 人 正 过 运动 学 


目前 ,工业 上 和 常用 的 机 器 人 机 械 结构 从 机 构 学 角度 看 大 多 属于 串联 机 构 , 对 其 进行 运动 
学 分 析 是 其 动力 学 .运动 控制 的 基础 。 串 联机 舌 人 运动 学 分 析 包 括 正 运动 学 问题 及 逆 运 动 
学 问题 。 机 器 人 运动 涉及 两 个 空间 , 即 关节 空间 和 任务 空间 的 映射 问题 ,机 器 人 末端 在 任务 
空间 运动 ,在 空间 的 位 置 与 姿态 由 各 驱动 关节 驱动 ,在 已 知 各 驱动 关节 位 置 参数 的 前 提 下 ， 
确定 机 严 人 末端 的 位 置 和 姿态 称 为 正 运动 学 ,可 简单 理解 为 运动 从 关节 空间 映射 到 任务 空 
间 。 正 运动 学 分 析 可 以 应 用 于 确定 任务 空间 大 小 、 标 定 机 器 人 几何 参数 等 方面 。 本 章 引入 
Denavit-Hartenberg 表示 方法 ,用 以 唯一 确定 各 相 邻 杆 件 的 位 置 和 姿态 ,从 而 描述 某 一 时 刻 机 
器 人 的 整体 结构 ( Architecture ) 或 者 是 位 形 (Configuration ) 。 


3.1 dic Ade 


如 图 3. 1 Brzn ,从 机 构 学 的 角度 看 ,串联 机 器 人 一 般 是 空间 开 链 机 构 。 通 常 第 一 个 杆 件 
称 为 机 器 人 基 座 (Base) ,最 后 一 个 杆 件 称 为 末端 执行 嚣 (End-effector，EE ) ,其 运动 链 (Ki- 
nematics Chain) ) 是 由 一 系列 杆 件 通过 不 同 的 运动 副 或 者 关节 连接 而 成 ,关节 连接 两 个 杆 件 ， 
限制 了 两 者 的 相对 运动 。 关 节 有 两 种 基本 形式 , 即 转动 副 R( Revolute) 和 移动 副 P( Prismat- 
ic) 。 如 有 果 一 个 关节 目 由 度 为 m(m>1) ,通常 拆 成 到 个 单 自由 度 的 R 副 或 者 P 副 ,其 连接 杆 
的 长 度 为 0。 各 驱动 关节 的 位 置 用 实数 表示 ,例如 ,转动 R 用 角度 表述 、 移 动 副 P 用 长 度 来 
表述 。 本 书 约定 各 杆 件 进 行 编 号 的 顺序 分 别 为 0,1,2,…,n, 各 关节 编号 顺序 为 1,2，…,m， 
其 中 第 i 个 关节 连接 第 i-1 和 第 i 个 连 杆 ,这 样 ,n 自由 度 的 机 器 人 由 n1 个 杆 件 和 个 关 
节 组 成 ,由 基 座 开始 任何 一 个 后 面 的 杆 件 都 通过 R 副 或 P 副 和 前 面 的 杆 件 相连 。 


连 杆 AT 


关节 1 





基 座 杆 0 末端 执行 器 
3.1 机 人 人 运动 链 
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3.2 Sis Aiax D-H 表示 法 


3.2.1 D-H 参数 的 几何 意义 


1. 单 杆 几何 参数 

分 析 机 妖 人 运动 链 中 的 第 i-1 个 连 杆 ,假定 i-1 连 杆 不 是 基 座 和 末端 的 连 杆 ,此 时 连 杆 
是 单独 的 刚体 ,其 前 后 端 连 接 第 i-1 和 i 关节 。 通 过 观察 可 以 发 现 , 无 论 连 杆 在 空间 如 何 运 
动 ,其 两 端 关节 轴线 的 距离 和 其 夹 角 不 会 发 生 改 变 ,因此 可 通过 此 两 个 参数 定义 两 条 轴线 的 
相对 位 姿 。 如 图 3.2 所 示 ,轴线 的 距离 度量 沿 着 轴线 的 公 垂 线 方向 ,从 i-1 轴 指向 i 轴 , 称 为 
连 杆 长 度 ( Link Length) ,用 符号 a; | 表示。 如 图 3. 3 所 示 , 假 设 存 在 一 个 平面 ,其 法 矢量 为 
两 轴 的 公 垂 线 的 单位 方向 ,其 方向 由 关节 i-1 轴线 指向 关节 i 轴线 ,两 轴线 投影 到 此 平面 ， 
遵循 右手 规则 ,其 角度 度量 从 关 市 i-1 轴线 方向 转动 到 关节 i 轴线 方向 ,此 角度 称 之 为 扭 角 
( Twist Angle) ,用 a; | 表示 。 





3.2 单 杆 的 几何 参数 图 
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图 3.3 扭 角 的 几何 意义 
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2. 相 邻 杆 件 几 何 参 数 

如 图 3. 4 所 示 , 相 邻 杆 i- 1 和 杆 i 有 共同 的 轴线 , 沿 看 此 轴线 正 同方 咎 ,两 连 杆 的 各 目 
两 端 轴线 的 公 垂 线 之 间 的 距离 ,定义 为 连 杆 偏 置 (Link Offset) ,用 符号 d; 表示 ,如 果 关 节 i 
是 一 个 转动 副 , 则 d; 是 常量 ;如 果 关 市 i 是 移动 副 , 则 d, 为 变量 。 两 连 杆 绕 共 同 的 轴线 转动 
的 角度 定义 为 关节 角 (Joint Aangle) ,用 符号 0; 表示 ,如 有 果 此 关 市 是 转动 副 , 则 9; 是 变量 , 即 
相 邻 杆 各 目 两 端 轴线 公 垂 线 的 夹 角 ;如 采 是 移动 副 , 则 0% 为 常量 。 

注意 ,各 几何 参数 下 标的 编号 要 一 致 ,如 单 杆 几何 参数 co,a- 与 杆 件 编号 一 致 ,变量 
d; 或 者 0; 表示 相 邻 杆 之 间 的 几何 参数 ,要 与 连接 两 连 杆 的 关节 编号 一 致 。 





3.4 相 邻 连 杆 的 几何 参数 


3.2.2 D-H 坐标 系 的 建立 


本 书 约 定 ,所 建立 的 坐标 系 F 固 连 于 连 杆 i-1 ,而 不 是 第 i 个 杆 , 其 中 i=1,2,…,n+l1。 
建立 笛 卡 儿 坐 标 系 要 确定 原点 的 位 置 以 及 各 坐标 轴 的 方向 ,如 果 机 器 人 机 构 含 有 n+l 个 连 
杆 , 则 坐标 系 建立 如 下 规则 : 

( 1) 首先 确定 各 坐标 系 z 轴 ,用 z 表示 了 坐标 系 的 z 轴 , 与 第 i 关节 的 轴线 重合 ,其 正方 
癌 按 照 表 示 方 便 的 原则 自由 定义 。 移 动 副 的 z; 轴 可 以 放 在 任意 位 置 ,因为 移动 副 只 有 方向 
的 定义 。 

(2) 在 确定 各 坐标 系 z 轴 位 置 和 方向 的 前 提 下 ,进一步 确定 坐标 系 F; 的 x 轴 方 向 和 坐 
标 原 点 ERD i71 和 关节 i 建立 的 z,, 轴 和 zz 轴 。 在 几何 上 ,其 空间 位 置 有 交叉 、 相 交 和 平 
行 三 种 情况 ,下 面 分 别 讨论 在 此 三 种 情况 下 如 何 建 立 坐 标 系 的 x 轴 和 原点 。 

1. 轴线 空间 交叉 

如 图 3. 5(a) 所 示 ,x; 垂直 于 zi 和 二 ,其 方向 从 za 指向 za ,0O; 09 x; 与 z 的 垂直 交点 。 

2. 轴线 相交 

如 图 3. 5( b) 所 示 ,如 果 z; A z 两 轴 相 交 , 不 能 按照 空间 交叉 的 情况 定义 x, 轴 的 正 向 ， 
此 正 回 可 以 按照 左手 法 则 或 右手 法 则 确定 ,从 z;_ 轴 转 动 到 zx 轴 , 拇 指 指向 的 方向 为 x; 的 正 
[5] ,O; 2S z; , 5 z; JA RS 
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3. 轴线 平行 

如 果 z; 和 z 两 轴 平 行 , 其 公 垂 线 有 无 数 条 , 则 xc; 轴 的 位 置 没有 定义 ,一 般 而 言 , 为 了 唯 
一 确定 x, 的 位 置 ,x; 应 过 原点 0; 。 但 在 特殊 境况 下 ,为 了 分 析 方 便 ,x; 沿 z; 平移 ,与 z; 交 
点 即 为 0;, 如 图 3.5(c) 中 虚线 所 示 。 





(c) 平行 情况 


图 3.5 相 邻 关 届 轴线 的 空间 位 置 
从 建立 坐标 系 的 过 程 可 以 看 出 ,F; 坐标 系 的 建立 依赖 于 Fi; | 坐标 系 的 信息 ,坐标 系 F, 
的 x 轴 方 向 和 原点 的 位 置 可 根据 分 析 人 简单 的 原则 目 由 定义 ,第 n+l 个 坐标 系 固定 在 第 n 个 
杆 件 的 后 端 ,因为 机 器 人 没有 n+l 个 关 市 ,zi 位 置 和 方向 无 法 定义 , 故 规则 不 适用 于 最 后 一 
个 坐标 系 的 定义 ,但 也 要 根据 分 析 问 题 方便 的 原则 ,自由 定义 出 合适 的 坐标 系 。 这 样 n+1 
个 杆 定 义 了 n+1 个 坐标 系 Fi Fast ,下 ,si ,它们 依次 固 连 于 杆 件 0,1,…,n 上 ,一 个 n 轴 开 环 
串联 机 器 人 机 构 包 括 nel 个 杆 件 和 n+1l 个 坐标 系 。 


3.2.3 D-H 参数 表 的 建立 


根据 坐标 建立 的 规则 ,在 杆 i-1 和 杆 i 建立 了 坐标 系 ,如 图 3. 6 所 示 , 按 照 定 义 的 D-H 
参数 所 表示 的 几何 意义 ,从 图 中 可 以 观察 出 以 下 参数 的 定义 。 

(EFKE. z 和 ,1 两 轴 之 间 的 距离 即 为 a, WA xi, 正 向 ,为 非 负 值 。 

(2) 连 杆 扭 角 。z 和 zi 两 轴 之 间 的 夹 角 即 为 w ,其 正 负 由 %,, 轴 确定 ,遵循 右手 规则 ， 
M. zi 转动 到 z1 0 

(3) 连 杆 偏 距 。x, 轴 与 z, 轴 的 交点 记 为 0';,0;0'; 的 距离 即 为 d;, 此 值 的 正 负 取 决 于 
00' 方 向, 如果 此 方向 与 = 轴 正 向 一 致 , 则 为 正 ,反之 为 负 。 从 图 3. 6 中 也 可 以 看 出 ,d; 的 
绝对 值 由 x 和 x, 两 轴 的 距离 确定 。 
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图 3.6 D-H 参数 示意 图 

(4) 关 节 角 o Xi 和 x 1 两 轴 之 间 的 夹 角 即 为 0, ,其 正 负 由 Zi 轴 确 定 ,遵循 右手 规则 ,从 Xi 
转动 到 xi。 

如 果 第 i 个 关节 是 R 型 , 则 0; 为 第 i 个 关 市 的 关 市 变量 (Joint Variable) , a; ya I d; 为 
常量 , 称 为 连 杆 参数 或 为 连 杆 常数 。 如 果 第 i 个 关 市 为 P 型 , 则 d; 为 关节 变量 ,其 他 量 为 连 
杆 参 数 。 因 此 ,n 轴 操 纵 手 有 nn 个 关 记 变 量 , 组 成 一 个 n 维 矢量 0 ,无论 是 转动 关节 还 是 移动 
关节 ,都 有 3n 个 连 杆 常数 。 连 杆 浓 数 确 定 了 操纵 天 的 结构 ,而 关节 变量 确定 了 操纵 需 的 位 
形 。 

【 例 3.1】 平面 3R 机 械 辟 坐标 系 的 建立 及 D-H 参数 表 图 3.7 描述 了 一 个 平面 3R 
机 械 辟 ,按照 建立 坐标 系 的 规则 ,所 建 坐标 系 从 F 开始 ,z; 各 轴 方 向 垂直 纸 面 向 外 , 且 相 互 
平行 ,x 选择 沿 水 平方 向 ,O01 定义 于 关节 1 回转 中 心 ,坐标 系 ,的 建立 规则 按照 轴线 平 
行 的 情况 建立 ,其 原点 恰好 位 于 各 关节 的 回转 中 心 ,x> ,xs 轴 的 方向 如 图 3.7 所 示 ,机械 臂 末 
端 坐标 系 Fa 的 z 轴 与 第 3 RT z 轴 平行 ,原点 位 于 手 爪 中 心 。 坐 标 系 建立 完成 后 ,D-H 参 
数 可 以 按照 上 述 方法 观察 得 到 ,其 格式 及 参数 见 表 3. 1。 





图 3.7 平面 3R 机 械 臂 简 图 
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表 3.1 平面 3R 机 械 辟 的 D-H 参数 表 


i d; Q; d, 0, 
] A 0 0 0, 
2 L 0 0 0, 
3 l 0 0 0, 


[9513.2] 平面 RPR SLEEP [53.8 描述 了 一 个 平面 RPR 机 械 臂 ,按照 建立 坐标 系 
的 规则 ,所 建 坐标 系 从 F, 开始 ,R 型 关节 = 轴 方 向 垂直 纸 面向 外 ,第 2 关节 =z 轴 方向 如 图 3. 8 
所 示 ,xi 选择 沿 水 平方 向,0, 定义 为 关节 1 回转 中 心 ,机 械 臂 末端 坐标 系 Fa 的 z 轴 与 第 3 
关节 =z 轴 平行 ,原点 位 于 手 爪 中 心 。 平 面 RPR 机 械 臂 的 D-H 参数 表 见 表 3. 2。 





图 3.8 平面 RPR 机 械 臂 简 图 
表 3.2 平面 RPR 机 械 臂 的 D-H 参数 表 


i a, a d, 0, 
l 0 90 0 0, 
2 0 90 d, 180 
3 E 0 0 0, 


3.2.4 D-H 坐标 系 之 间 的 齐 次 变换 


坐标 系 F, 固 连 于 杆 i71 的 后 端 ,坐标 系 Fi EETA i 的 后 端 ,显然 两 杆 之 间 的 相对 位 
置 和 姿态 可 以 通过 两 个 坐标 之 间 的 齐 次 变换 矩阵 来 描述 ,下 面 详细 讨论 这 两 坐标 系 之 间 的 
姿态 和 位 置 的 关系 。 

通过 观察 可 知道 ,影响 两 个 坐标 系 姿态 的 是 两 个 角度 , 即 0; AA a 角 , 参 考 图 3.6, 得 
出 三 个 相 邻 坐标 系 关系 如 图 3. 9 所 示 ,假定 初始 F, Fi,, 两 坐标 重合 ,可 以 通过 有 序 的 两 次 
旋转 得 出 两 坐标 系 的 方位 关系 ,从 Fi 方向 变换 成 i, 方向 ,如 图 3.9 所 示 。 首 先 , 绕 坐标 系 
zi 轴 旋 转 0; 角 , 其 后 绕 xs 旋转 ai ,两 次 旋转 都 是 绕 坐 标 系 Fi, 进行 的 。 因 为 扭 角 a 为 常 
量 ,为 了 书写 方便 ,首先 令 

人 ;=COS Qi ,Mi;=sin a; 


这 里 特定 令 Q; 表示 D-H 方法 中 Fi;,, 相 对 于 F, 的 旋转 矩阵 , 写 为 
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图 3.9 ” 相 邻 三 个 坐标 系 的 位 姿 关 系 
cos 0, —A.sin 0; ;sin 6, 
‘Q -Q,-Rot(z,0,) Rot(x,o;)-| sin 0; Aicos 0; -hicos 6, (3.1J 
0 fli À; 
如 果 令 ,3 和 六 ,表示 坐标 系 已 ,的 各 坐标 轴 在 坐标 系 F, 的 单位 方向 ,从 图 3. 10 
中 也 容易 得 出 


cos B), Wisin 6, —AÀ;sin B, 
PA =| sin 8, |, pA =| -4,cos 0; |, EN =iz,, xi =| A;cos B, UL a) 
0 À; 从; 
与 @; 矩阵 中 三 个 列 矢量 所 表示 的 两 个 坐标 系 方 同意 义 一 致 。 
epi... he 





图 3. 10 各 坐标 系 之 间 的 齐 次 变换 矩阵 


下 面 推导 两 个 坐标 系 之 间 的 位 置 关系 。 参 考 图 3. 6 ,在 坐标 系 F, 下 ,坐标 系 ,原点 的 
位 置 用 矢量 a; 表示 ,从 图 可 以 得 
0 cos 0; Qicos Q; 


—— — P y 
a; -0;0,,, -0,0',*0';0,, -d/z;*a;' Xi 2d; | 0 | +a;| sin 0; | =| a;sin 0; (3.3) 


l 0 d, 


t 
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坐标 系 Fa 相对 于 坐标 系 F 的 位 置 和 姿态 通过 a; 和 Q; 确定 ,可 以 用 其 次 变换 矩阵 工 , 
表示 为 





cos 0, -A.sin 0; Wsin 0;  ajcos Q; 
sin 0; Acos0 -icos0 asn 0; 
ra | a D (3.4) 
0 ji 0 Mi À; d, 
0 0 0 l 


参考 第 2 章 的 知识 ,并 参考 图 3.9 所 示 的 两 坐标 系 Ff, 与 F;, 自 重合 的 位 置 开始 ,F,,, 在 
自身 坐标 系 下 , 沿 z 轴 平 移 d;, 绕 z 轴 旋 转 9; 角 , 然 后 沿 x 轴 平 移 a; , Bn Z6 x 轴 旋 转 o; 角 ， 
经 过 四 次 变换 ,得 出 两 坐标 之 间 的 齐 次 变换 矩阵 ,其 过 程 可 以 写 为 

T,-Trans(0 0 d;)Rot(z,0;)Trans(a; 0 0)Rot(x,a.,) (3.3) 

计算 最 终 得 到 的 矩阵 也 与 式 (3.4) 一 致 。 显 然 ,D-H 参数 表 中 第 i 行 的 参数 代入 式 

(3.4) 所 得 到 的 齐 次 变换 矩阵 ,唯一 确定 了 Fi,, 相 对 于 FF; 的 位 置 与 姿态 关系 。 


3.3 Haa AEE 


一 旦 建立 机 械 臂 各 杆 坐 标 系 ,相应 的 D-H 参数 表 得 到 后 ,就 可 以 很 方便 地 分 析 机 器 人 
的 正 运动 学 ,机 和 从 人 末 端的 第 nl 个 坐标 系 在 坐标 系 |11 的 齐 次 变换 矩阵 可 以 通过 各 齐 次 
变换 矩阵 连 乘 得 到 , 即 
qae Ts T Ten (3.6) 
各 个 坐标 系 之 间 的 位 置 与 姿态 可 以 通过 齐 次 变换 矩阵 相 乘 得 到 ,如 图 3. 10 所 示 。 
机 和 硕 人 末端 的 姿态 和 位 置 也 可 以 写 为 
0-0;0,---Q, 
| (3.7) 
nli clip PORRO RS 
式 中 ,Q Mp 分 别 表示 在 坐标 系 F 下 ,末端 坐标 系 ,i 的 姿态 与 位 置 ;a, 表示 在 坐标 系 F, 
下 坐标 系 Fi 原点 的 位 置 拓 量 。 注 意 ,比较 式 (3.6) 和 (3.7) ,前 者 易于 理解 ,而 后 者 计算 方 
便 , 式 (3.6) 计 算 4x4 和 矩阵 的 16 个 参数 , 式 (3.7) 仅 需 计 算 12 个 参数 ,9 个 姿态 矩阵 参数 ,3 
个 位 置 矢量 参数 。 在 机 右 人 正 运动 学 分 析 中 , 式 (3.6) 和 (3.7) 右 侧 是 关节 变量 的 输入 , 左 
侧 是 结果 的 输出 。 
【 例 3.3】 平面 3R 机 械 臂 正 运动 学 平面 3R 机 械 臂 机 构 如 图 3.7 所 示 ,D-H 参数 表 
见 表 3. 1。 齐 次 变换 矩阵 T,(i=1,2,3) 可 以 通过 式 (3.6) 得 到 , 则 此 机 械 臂 的 正 运 动 学 可 以 
表示 为 


Tef LF, 
AF, T 表示 末端 坐标 系 相对 于 坐标 系 111 的 齐 次 变换 矩阵 ,推导 得 
Ca  —$53 0 de, tbc; t0 
T- S €p3 O0 ls bs ls, (3.8) 
0 0 1 0 
0 0 0 1 


IP Sia =S (0, 0,0,) ,ci23 =cos (0,*0,*0,). 
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【 例 3.4】 平面 RPR 机 械 辟 正 运 动 学 平面 RPR 机 械 臂 机 构 如 图 3. 8 所 示 ,D-H 参 
数 表 见 表 3.2。 各 坐标 系 之 间 的 齐 次 变换 矩阵 T,(i=1,2,3) 可 以 通过 式 (3.6) 得 到 , 则 此 机 
械 臂 的 正 运动 学 可 以 表示 为 

FsT LIT. 

式 中 ,了 表示 末端 坐标 系 相 对 于 坐标 系 11 | 的 齐 次 变换 矩阵 ,推导 得 
C3 753 0 dc hey 
S3 C3 0 djs 
0 O0 1] 0 
0 0^ 0 l 

【 例 3.5】 空间 6R 机 械 臂 正 运动 学 图 3.11~3.13 所 示 为 空间 6R 机 械 臂 ,包括 七 
个 杆 件 ,编号 为 0 ~ 6 ,七 个 坐标 系 ,编号 为 1 ~7, 最 后 一 个 坐标 系 可 以 任意 定义 ,一 般 而 言 ， 
其 z 轴 方 回 与 最 后 一 个 关节 所 定义 的 z 轴 方向 一 致 ,其 原点 位 于 机 械 臂 末端 执行 器 设 定 的 
操作 点 (在 任务 空间 描述 机 械 臂 末端 运动 的 参考 点 ) 处 ,第 1 关节 和 第 2 关节 轴线 垂直 ,第 2 
关节 和 第 3 关节 轴线 平行 ,最 后 三 个 关节 轴 相 交 于 一 点 C, 组 成 机 械 臂 的 手腕 ,前 三 个 关节 
运动 改变 C 点 的 位 置 ,而 后 三 个 关节 运动 时 ,点 C 保持 不 动 。 当 点 C 保持 不 动 时 ,手腕 上 所 
有 点 以 上 为 球 心 运动 , 称 为 球形 手腕 ， 此 机 械 臂 结构 为 解 耦 型 结构 ,在 D-H 参数 中 ,a, = 
as =a6=0, F; ,Fe 坐标 系 原 点 重合 。 在 图 3. 11 ~3. 13 中 , 构 型 1 、 构 型 2 和 构 型 3 存在 细微 
的 差别 ,图 3. 11 大 辟 存 在 偏 距 , 即 杆 1 和 杆 2 有 连 杆 偏 置 d, ,图 3. 12 除 存 在 大 臂 偏 置 外 ,小 
臂 存 在 连 杆 长 度 a, ,图 3. 13 大 辟 无 偏 置 ,但 杆 1 具有 长 度 a, ,所 建立 的 坐标 系 原点 的 矢量 
线 , 即 从 O, ~ 06 的 连 线 在 同一 个 平面 上 ,可 以 降低 运动 学 计算 的 难度 。 根 据 所 建立 的 坐标 
系 , 得 到 D-H 参数 表 见 表 3.3 ~3. 5。 


(3.9) 





图 3.12 T4372 
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图 3.13 构 型 3 

表 3.3 构 型 1 的 D-H 参数 表 
L Qt a; d; 0, 
l 90° 0 d, 0, 
2 D" a d. 0, 
3 90? 0 0 0, 
4 -90° 0 d, 0, 
5 90? 0 0 0, 
6 0° 0 d, 6. 

表 3.4 构 型 2 的 D-H 参数 表 
l Qt; a; d; 0, 
1 90° 0 d, 0, 
2 0 G^ d, 0, 
3 90? a, 0 0, 
4 -90° 0 d, 0, 
5 90。 0 0 6. 
6 0 0 d, 0; 

表 3.5 构 型 3 的 D-H 参数 表 
l Qt; a; d; 0; 
l 90° Qi 0 0, 
2 0 à, 0 0, 
3 90? a, 0 0, 
4 —90? 0 d, 0, 
5 90? 0 0 05 
6 0 d, 0, 


以 上 三 种 构 型 的 机 船 人 机 构 ,重点 对 图 3. 13 的 机 构 进行 正 运 动 学 分 析 , 根 据 式 (3.4) 
和 D-H 参数 表 ( 表 3. 5) 的 连 杆 参数 ,可 求 得 各 坐标 系 相 对 姿态 和 位 置 为 


& U — Q, C, 
Qi=|s 0 -ci| ,Ar=| a 
Oo 1 0 0 


Q:=| s2 c, 0| ,a,=| as, 


0 0 |] 0 
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e U $ (46; 
Q,-|s, 0 -cs| ,03=| azs, 
B=W 0 0 
C4 f 0 
Q, -| s, C4 |,04,-— 0 
0 -1 0 d, 
&p- Uo Si 0 
Q: =| s; -Cs | :05 三 | 0 
0 0 0 
Cc; -s 0 0 
Qe=|se c, 0l,a6=| 0 


© 

< 
a 

CN 


式 中 ,c, 表示 cos 0,;s, 表示 sin 0,。 
各 连 杆 齐 次 变换 和 矩阵 相 乘 ,得 到 机 械 手 在 基 上 坐标 系 下 的 齐 次 变换 矩阵 为 


I-T,(0,) T2(0,) T4,(0,) T,60,) T4,(C0,) T5(0,) (3. 10) 
即 为 关节 变量 0 ,0 ,… ,06 的 图 数 ,为 了 求解 此 方程 ,要 把 一 些 中 间 结 果 计 算出 来 。 
C5Cg ”一 C536 55s dess 
S.C, S:S; 一 C- -d:c 
he (3.11) 
Si Cs 0 0 
0 0 0 l 
CaCsC6 84s6 C4C5S67S4C6 — Cas | d 6,5; 
$,C.C&.--CAS& — —S4C«S& 二 cc Eds did 
iT z^ TT, = 4CsC6 * C486 Ax Saig | Memes (3.12) 
=S; C6 Bf. Gs | d OUO, 
0 0 0: 1 
XT 2 和 关节 3 互相 平行 ,得 
c 0 s3 azcz+Q3C23 
T,= °T 37 = S3 0 -cs 0252 十 03323 (3.13) 
0 . 1 0 0 
0 0 0 1 
CiC3 $i Cisa | ci ( a, 056; *a5€5, ) 
$1C —C, Sisa 1S, 04-05, toscss ) 
'T,-'T/T,- 1€23 1 23 I\ a; F05€, 04€ (3. 14) 
$23 Ü  -— 1 Q232 十 Q3323 
0 0 0 : 1 


机 械 辟 手 爪 坐标 系 相 对 于 基 坐 标 系 得 齐 次 矩阵 为 
T= r "T 
机 器 人 机 构 构 型 众多 ,最 终结 果 无 必要 展开 ,读者 可 以 自行 计算 得 到 。 手 腕 中 心 点 的 位 


置 定义 为 pc ,根据 式 (3. 14) 可 以 计算 为 
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0 
0 | = 
0,5, t 04$, d, 


为 了 校 核 7= TT 是 否 正确 ,给 定 机 械 臂 初始 状态 ,各 关节 变量 的 值 为 
= 0 ,9; 20^,0,—0*,8. :0*,8, 307 
坐标 系 原 点 位 置 矢量 线 如 图 3. 14. 所 示 ,此 时 杆 2 与 水 平面 垂直 , 杆 3,4,5,6 平行 于 水 
平面 ,计算 T S 


Pc -po,* Qa, = 


c, ( a4 *a5c, 24€, ) Cl1C23 S,  C4$5 


si(a; +a,C,+a3C3 ) |+| S1C23 -C1 81525 s, (a, +a,C, ta5c5 *d,55, ) 


c, (a, *a5c, 0404, *d45,, ) 
ES. 15) 




















$53 0 一 C23 aS, * 0455, 04€); 


0 0 1 a,-*d,*d;, 
0 -1 0 0 

T= 
EE 09 a, +4, 
0 0 0 l 





图 3.14 初始 位 置 坐标 原点 矢量 线 


3.4 dizi AERA 


3. 3 节 建 立 的 机 事 人 末端 位 姿 ( 位 置 和 姿态 ) 与 机 器 人 各 驱动 关节 变量 的 表达 方式 , 称 
为 机 妖 人 正 运 动 学 ,机 器 人 逆 运 动 学 是 在 给 定 末 端 位 姿 的 前 提 下 ,求解 相应 各 个 关节 变量 。 
机 第 人 逆 运 动 学 分 析 在 机 伸 人 运动 控制 中 是 一 个 重要 问题 ,因为 机 器 人 末端 期 望 的 轨迹 是 
通过 关节 运动 实现 的 。 逆 运动 学 复杂 度 较 高 的 原因 如 下 : 

(1) 解 方程 过 程 涉及 方程 非 线 性 问题 ,有 时 无 法 推导 出 解析 解 。 

(2) 存 在 多 解 的 情况 。 

(3) 在 茶 些 特殊 位 形 下 存在 无 穷 解 ,如 机 右 人 处 于 奇异 位 形 或 者 元 余 驱 动 时 。 

机 船 人 逆 运 动 学 求解 析 解 通常 采用 几何 法 和 代数 法 ,虽然 在 机 器 人 在 实时 控制 时 希望 
机 癸 人 存在 解析 解 ,但 不 是 所 有 的 机 器 人 逆 运 动 学 都 存在 解析 解 。 存 在 解析 解 的 机 器 人 在 
所 有 构 型 中 只 占 极 少 部 分 ,一 般 而 言 ,工业 上 应 用 的 机 器 人 大 部 分 存在 解析 解 。 几 何方 法 利 
用 机 器 人 机 构 特别 的 结构 形式 ,采用 几何 观察 的 方法 求 各 个 关节 变量 ,直观 但 不 严谨 。 代 数 
法 利用 式 (3.6) 中 的 代数 变换 求 各 个 关节 变量 ,有 时 需要 两 种 方法 结合 起 来 求解 。 通 常 , 末 
冰 的 位 置 和 姿态 可 通过 以 下 两 种 形式 给 出 : 
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r(e E 


式 中 ,T 表示 末端 在 机 器 人 基 坐 标 系 下 的 齐 次 变换 矩阵 ;末端 的 位 置 以 矢量 p 给 出 ;姿态 以 
旋转 矩阵 Q 给 出 ;x。 列 和 矩阵 中 qo 表示 绕 基 坐标 系 或 工具 坐标 系 三 个 坐标 轴 的 旋转 角 , 如 可 
采用 RPY 角 描 述 未 端的 姿态 。 


3.4.1 平面 3R 机 械 辟 逆 解 


图 3. 15 为 平面 3R 机 械 臂 ,给 定 末端 手 爪 位 置 与 姿态 为 x.=[x。 Ye Pelo e. HFR 
绕 z 轴 的 转动 角度 ,求解 关节 角 0, ,9 和 0,。 





图 3.15 平面 3R 机 械 臂 


1. 3R 机 械 辟 几何 法 

经 过 观察 可 以 发 现 , 当 0, 运动 时 ,C 点 的 位 置 保持 不 动 ,因此 C 点 的 位 置 仅仅 依赖 于 
0, ,0, 的 值 ,从 图 3. 15 中 可 以 知道 ,C 点 的 位 置 为 
oy p 


; (3. 16) 
yc — y,-l,sin 9 


D 


应 用 三 角 函 数 定 理 , 得 





r^ =} «l-21l,L,cos B D. I7) 
式 中 Ka =x yc , 则 进一步 得 
li+l—r ; 2 
cos B= JN. =D sin B=+vV 1-D (3. 18) 
1*2 
成 立 , 有 一 个 约束 条 件 , 即 
r<l +l, 


当 手 腕 中 心 点 位 置 超出 机 械 臂 的 可 达 工 作 空间 , 式 (3. 18) 不 成 立 。 假 定 约束 条 件 成 
立 , 则 关节 角 0, 可 以 表示 为 
0,=7-B=7-atan 2( lJ D 
显然 , 当 0, e (0,7] 时 ,3R 机 械 臂 肘 部 下 降 (Elbow-down) ,而 当 6; s[-T,0) 时 , 肘 部 抬 
起 (Elbow-up), ?4 8, ZO 时 ,关节 2 处 于 过 渡 状 态 ,此 时 机 构 位 形 奇 异 (Configuration Singular- 


50 工业 机 器 人 技术 


ity) 。 已 知 C 点 的 坐标 为 (xc,yc) ,图 3. 15 中 a 角 可 以 表示 为 


a —-atan 2( yc ,xc) (3. 19) 
y 角 可 以 表示 为 
y 2 atan 2( lsin 0, ,li+l,cos 0， ) (3. 20) 
0, 角 可 以 表示 为 
0, -a.—y (3.21) 
0, -49—0, —0, (3. 22) 
因为 2 有 两 个 解 , 当 给 定 3R 机 械 臂 位 姿 时 ,其 逆 解 有 两 组 解 。 
2. 代数 法 


根据 正 运动 学 分 析 ,3R 机 械 臂 的 末 痊 坐标 系 相 对 于 基 坐 标 系 的 齐 次 矩阵 如 式 (3. 8) Br 
示 , 和 矩阵 中 第 四 列 前 二 行 分 别 对 应 于 末端 的 位 置 。 手 腕 绕 z 轴 转 动 ,其 旋转 矩阵 可 以 参考 式 
(2. 21) ,与 式 (3.8) 所 示 的 齐 次 矩阵 比较 ,可 以 得 出 


sin Q-25,5 
C08 QC, 
t Slt toes Leu, (3.23) 
y, 51,2, 5, $5 
参考 图 3. 15 ,利用 式 (3. 23) ,可 以 进一步 得 出 
dial. Elis, (3.24) 
yo ils, s.s 
XX (G. 24) 两 边 平 方 , 然 后 相 加 ,得 
xy. zl + 2l, Lc, (3. 25) 
解 方程 (3. 25) ,得 
xctyc-li -h 3 
GT ap Vl (3. 26) 
因此 6. 为 | 
0,-atan 2(5,,c, ) (3.27) 
由 06, 已 知 , 式 (3.24) 重 新 写 为 
二 (3.28) 
ye Cl the, ) s, -F(Ls, )e, 
A 
fe, zd, Lus, 
k, Ls, 
采用 三 角 代 换 法 
k, 2pcos y 
We (3. 29) 


式 中 ,p=Vhi+tk ,将 式 (3. 29) 代 入 式 (3. 28) ,得 


Xç =pcos yc, -psin yc, ,Xe 2pcos(y40, ) 
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yc 7pcos ys, *psin yc, , y, 2psin( y40, ) 
则 % 可 以 表示 为 
0, 2atan 2( yc ,x,. ) -atan 2( kki) 
0, — 9-0, -0, 


3.4.2 空间 6R HRES D fE 


空间 6R 机 械 臂 求 其 逆 解 非常 困难 ,但 机 需 人 在 腕 中 心 点 (Wrist Center) C 解 看 ,如 图 
3. 11 所 示 ,那么 , 逆 解 问题 可 以 分 解 成 位 置 逆 解 和 姿态 逆 解 两 个 相对 简单 的 问题 。 如 果 机 器 
人 未 端的 位 置 和 姿态 在 坐标 系 11| 下 给 定 为 
;Q(0,,…,0)=0Q 
jp(01,…,06)=p 
式 (3.30) 中 ,Q 和 pp 为 已 知 的 姿态 和 位 置 , 逆 解 问题 是 求解 9, ,… ,90。。 参 考 图 3. 11,0 
第 三 列 为 z 轴 在 坐标 系 11 下 的 单位 方 回 ,可 以 知道 
0 
0 


(3. 30) 


p-p.*d,Q (3.391) 








如 果 将 给 定 的 O 和 p 表达 为 
e T) ^ 2H (3.32) 
Xe Xe 一 d6ri3 
z pee 
Zc Ze 一 d6733 
利用 已 知 的 C 点 的 位 置 可 求解 0 ,9 和 0 , 称 为 位 置 逆 解 。 
1. fr E ARR ( 几何 法 ) 
以 不 存在 大 辟 偏 置 的 机 器 人 机 构 为 例 ,图 3. 16 为 空间 3R HRE 79 Y ARD RI 


算 难 度 ,由 机 械 臂 坐标 系 Fi ~ Fs 原点 所 连 成 的 矢量 线 位 于 同一 平面 ,投影 此 矢量 线 到 xy F 
面 ,此 时 0, 可 以 表示 为 


则 胸中 心 点 的 位 置 天 量 pi 为 


(3,33) 








0, 2atan 2( yc,x«) (3. 34) 
存在 第 二 个 可 能 解 为 
0, 2ar*atan 2( yo ,x() (3, 39) 
当 xc=yc=0 时 , 即 C 点 位 于 za 轴线 上 ,6 不 确定 ,机 构 处 于 奇异 位 形 。 当 存在 大 臂 偏 
置 时 ,参考 图 3. 11 ,此 时 d, 250 , 则 腊 中 心 C 点 可 以 避免 相交 于 zi 轴 , 以 坐标 系 111x 轴 正 向 
为 分 界线 ,可 分 为 左 臂 (Shoulder Right) 和 右 臂 (Shoulder Left) 两 种 情况 ,在 水 平面 投影 后 ， 
对 应 的 位 形 如 图 3. 17 和 图 3. 18 所 示 。 
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图 3.17 左 臂 位 形 图 3.18 右 壁 位 形 


对 于 左 避 的 情况 ,从 图 3. 17 中 几何 的 关系 可 以 看 出 
0, =a-Yy 


式 中 
a —atan 2( yc, Xç) 
y -atan 2(d, alr -dy j= atan 2(d, a 2e tye -d3 ) 
XX (. 36) 进 一 步 写 为 
0, =atan 2( yc ,xc.) —atan 2(d, ,J/xz*y1—d, ) 
对 于 右 臂 的 情况 ,从 图 3. 18 中 的 几何 关系 可 以 看 出 
0; =a+B 
Am 
a —atan 2( yc ,x,.) 
P=n+y 


y=atan 2(d, ,/ x7 y, -d, ) 


(3:36) 


(2. 37) 


(3.38) 


(3. 39) 


(3.40) - 
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式 (3. 40) 可 以 进一步 写 为 


0, =atan 2(yç,%ç)+atan 2(d, ,A/ xz *yz—d5 ) +r 
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0, 已 经 求 出 ,下 面 求解 0, 410,5 A TORTE 0, 和 0,, 沿 图 3. 17 的 投影 方向 ,把 杆 2 和 杆 
3 投影 到 z, IURI xc, 轴 组 成 的 平面 上 ,如 图 3. 19 所 示 ,9, 和 9, 求解 类 似 于 平面 2R 机 构 。 


An 


因此 


Z] 





X5 


图 3.19 杆 2 和 杆 3 的 投影 


0, = 2 


».5 2 
民 +s’ —a5-d, 
o a ——— — —— JH 


pA 


Rz xt *y,—d; 


a=atan 2( £/ 1-D^ ,D) 


0, 2 atan 2(s,R) -atan 2(d,sa , a, *d,ca) 


2. 姿态 的 逆 解 (代数 几何 结合 法 ) 


D, -0,0.,0, =M 


ARE M 可 以 通过 以 下 计算 得 出 : 


M-(Q.)' (Q)' (Q.))'0-(9,)'Q 


经 计算 ,假设 矩阵 M 在 坐标 系 F 下 表示 为 


(3. 41 ) 


a 的 两 个 解 分 别 对 应 于 肘 部 落下 (Elbow-down) 和 肘 部 抬 起 (Elbow-up) 两 个 位 置 。9， 
求解 为 


(3. 42) 


0, ,0; 和 0 已 经 求 得 , 则 已 知 旋转 矩阵 8,,Q, 和 Q, ,坐标 系 到 相对 于 坐标 系 Fa 的 旋 
转 和 矩阵 Q, 表示 为 


(3. 43) 


(3. 44) 
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Mı m; mi; 


M- 


Tm, My My 








首先 定义 关节 的 轴线 单位 矢量 为 e; ,其 正 向 与 所 定义 的 z; 轴 方 同一 致 。 图 3. 20 所 示 
为 手腕 的 一 般 结构 ,三 条 转动 轴线 相交 于 一 点 ,由 角 as I os 定义 ,设计 值 不 能 为 0" 和 180°。 





图 3.20 手腕 的 一 般 结 构 
从 图 3. 20 中 可 以 看 出 ,关节 6 的 轴线 方向 es 5; A RF; 的 z 轴 方向 一 致 ,在 坐标 系 
F, 下 ,ec,es 可 以 表示 为 








Has Ma34 
4e = Hy, Z (3. 45) 
my À4 
则 "Be * e ds 
Tasa $4 -Maza tM Àg =As (3. 46) 
A 2 mua 
SI Bzmau, , 则 
C-A,-m3A, 
As,—Bc, =C (3. 47) 
利用 三 角 代 换 , 有 
4=pcos o,B -psin o (3. 48) 


式 中 ,p=VA -B* ,p=atan 2(B,A) , HERC. 47) JE AGR(G. 46) ,得 到 0, 的 解 为 
sin(8,-9) - C/p,cos(0,-9)* £V/1-(C/p)^ 
0,- -atan 2( C/p, /1-( C/p)? ) (3. 49) 
0, 2 atan 2( B,A) «atan 2( C/p , &/ 1-( C/p)? ) 
进一步 求解 0, , 式 (3. 43) 可 以 改写 为 





mi, mj; mj 
Q.-(Q,)'|m,; m; m; [t0 3^ 3. 50) 
T, M32 M33 
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根据 旋转 矩阵 Q, 的 特点 ,其 第 三 行 不 包含 9,, 因 此 式 (3. 50) 右 侧 乘 积 所 得 到 的 矩阵 中 
第 三 列 不 包含 %。 令 两 侧 矩 阵 的 第 三 列 前 两 项 相等 ,得 


esiti Moe e PB (3.51) 
-5c5 = - A (mq mj Àg ) S4 +À 4 (my tm; À 6 ) C4 teta ( mate tms A) 
由 于 已 知 0, ,通过 式 (3. 51) 可 以 计算 得 到 9;。 最 后 ,Q6 可 以 表示 为 
BW Aip 
Q,-Q;Qi;| m, Mn m; [9 543 
M3 M3 ma 








通过 式 (3. 52) 可 以 计算 得 到 656。 对 于 每 组 (0 ,90;) 可 得 到 对 应 的 6 值 ,因此 姿态 的 道 
解 有 两 组 解 。 

3. 代数 方法 

参考 图 3. 13 ,用 代数 方法 解 此 机 器 人 结构 的 逆 解 ,如 果 已 知 末端 的 位 置 和 姿态 ,利用 式 
(3. 33 ) 求 得 腊 中 心 点 的 位 置 ,再 利用 式 (3. 15) 可 以 得 到 腕 关节 中 心 点 位 置 和 关节 变量 0, ， 
0, 和 9, 的 表达 式 为 


%c=0( a *a5c, *a4c4,d,s5,) (3:33) 
yc 75, (a, +4,63 +4323 *d,55, ) (3. 54) 
zc 70585-04854 —d4C3, (3.33 
具体 求解 步骤 如 下 。 
(1) 求 9。 
从 式 (3.53)、(3.54) 可 以 看 出 ,如 果 a, +ac+aacz d, 天 0 , 则 
0, »atan 2( yc we) (3. 56) 
为 一 个 解 为 
0, =m+atan 2(Y¢ ,xe) (3. 57 ) 
(2) 求 0,。 
式 (3. 53) 两 边 乘 c, , 式 (3. 54) 两 边 乘 s , 相 加 得 
XcC, *yc$, 20, 二 Q2C? 十 03C2 十 04S23 (3. 58) 
故 
XcC, *yc5; 7G, 205€; ta4C5, *d45,, (3. 59) 


4 kz-xcc, *ycs,-a, , il X (3. 55 ) 和 式 (3. 59) 两 侧 平方 后 相 加 ,得 


] 2 2 2 2 2 
2 


A À szg (Pte di) ,利用 三 角 代 换 方法 ,0, 的 解 为 


0, =atan 2( d, ,a4) —atan 2( xa; d; -A! JA.) (3. 60) 
(3) 求 9,。 
为 了 求解 9,, 式 (3.55) 和 式 (3. 59) 重 新 写 为 
02C2 = K-05C5, 0455, (3. 61) 


05$) 22,—0485, *d4C4, (3. 62) 
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式 (3. 61) 和 (3. 62) 末 方 相 加 ,经 重新 整理 得 


] 8 2 
( kd4 +zca3 ) S23 +( Ka; 一 zcCd4 ) c, = 5 K +zc +a; +d4 一 02 ) 


m= kds 二 ZrG3 


4j 7 Ks 7st, ,利用 三 角 代 换 法 ,bs 可 以 求解 为 


l 2 
= 5 K^ tz. 4-2 d, —a; ) 


0,, =atan 2( I, £/ m^ +n -Ê ) -atan 2( n ,m) 


则 
0; = 03 —0; 
根据 式 (3. 12) ，@, 表示 为 
Cille Sase Cals Sgu Cass 
3g. = $4,C5Cg *C4$5 一 55C555 十 CA。 345; 
7$5€6 $536 Cs 
矩阵 M 可 以 计算 为 
人 fu, Mia nm 
M-(Q,) Q- 51 E 0 | 二 | Th Tus Mig 
€1393 StS —€23.] L7. Ts T3 M3; M32 M33 
比较 式 (3. 65) 和 式 (2. 23) 可 以 看 出 ,如果 令 
0,-a 
0; =B 
从 = 了 


(3. 63) 


(3. 64) 


(3.65) 


两 者 完全 相同 ,求解 % ,6 和 0, 的 过 程 可 参考 z-y-z 角 的 求解 过 程 ,也 可 按照 以 下 步骤 求 


解 。 
(4) 求 解 0, 


C485 = C1 C3713 S1 C2323 十 S23733 = Ila 


$485 二 3S1713 一 C1723 — II, 


如 果 $5 20 , 则 


0, = atan 2( Maz ,TN13 ) 


如 果 s;<0, 则 


0, =atan 2( -m , -m,44) 


如 果 s, 20,0, 根据 任务 自己 定义 。 
〈5) 求 解 9;。 


'Q;-(Q,)* (Q))'Q 


参考 式 (3. 11) , 则 O, 表示 为 


CsCg ”一 C536 $5 


5 
Q; = $sC6 —$58g  —Cs 


S6 6s 0 


(3. 66) 


(3. 67) 


(3. 68) 


第 3 章 ， 机 器 人 正 逆 运动 学 


C4 $4 0 m 
(Q,)' ((9Q,)'Q-(Q,)'M- 0 0 -lj||m; 

-54, c. 0 

则 
$5 三 C471113 54104 一 C5 = 一 77233 
0; =atan 2( c,m,4*s,mj, ,m4, ) 
(605K ft 0,. 
36 三 一 5477111 十 C477221 


Cg 三 一 S47212 十 C477222 


06 =atan 2( —s,m,; tc,m;, ,一 S477212 十 C477222 ) 


经 过 以 上 步骤 , 解 出 0, I 0s o 


本 章 小 结 


SJ 
M2 Miz 
My m» (3.69) 
TW, M33 
(3. 70) 
(3. 71) 


本 章 诈 先 研 究 了 D-H 方法 中 各 参数 的 几何 意义 。D-H 坐标 系 的 建立 以 及 各 D-H 参 
数 的 选取 ,深入 分 析 了 两 相 邻 坐标 系 的 位 置 与 姿态 的 关系 ,进而 利用 D-H 方法 分 析 了 常见 
空间 6R 机 械 臂 的 正 运动 学 。 采 用 几何 法 、 代 数 法 分 析 了 平面 3R 和 空间 6R 机 械 臂 的 逆 运 


动 学 问题 。 


第 4 章 速度 与 雅 可 比 和 矩阵 


第 3 章 对 正 逆 运 动 学 方程 建立 了 关节 变量 与 末端 位 置 和 姿态 的 关系 ,本 章 研 究 关 节 速 
度 、 末 端 线 速度 与 角速度 的 关系 ,两 者 之 间 的 关系 可 以 用 一 个 矩阵 表示 , 称 为 雅 可 比 和 矩阵 , 雅 
可 比 矩 阵 在 分 析 机 占 人 机 构 特 性 方面 有 着 重要 的 作用 ,体现 在 以 下 几 方 面 :了 机构 位 形 的 奇 
异性 分 析 ; 人 GO 驱动 元 余 分 析 ;G) 可 用 于 逆 解 的 数值 解 方法 中 ;多 描述 作用 于 关节 力 与 作用 于 
未 端 力 之 间 的 关系 ;应 用 于 机 器 人 力 控制 。 本 章 首先 从 刚体 在 空间 运动 的 一 般 情况 分 析 
其 速度 与 加 速度 的 表达 形式 ,然后 建立 工业 机 磊 人 的 雅 可 比 矩 阵 。 


4.1 刚体 的 运动 速度 与 加 速度 


4.1.1 刚体 上 点 的 速度 和 加 速度 分 析 


刚体 在 空间 运动 可 以 看 成 与 之 固 连 的 坐标 系 |B| 的 原点 平移 运动 和 刚体 绕 其 原点 的 旋 
转运 动 ,在 惯性 参考 坐标 系 14} 下 ,刚体 上 所 有 点 的 位 置 、 速 度 和 加 速度 可 以 表示 成 关于 时 
间 的 函数 。 如 图 4.1 所 示 , 在 坐标 系 |1B| 下 的 点 5 在 坐标 系 |4| 下 的 位 置 撩 量 可 以 表示 为 

‘ps= Paonc*R rs (4.1) 





图 4.1 点 5 在 刚体 上 静止 


为 了 方便 书写 ,在 坐标 系 |41 下 的 矢量 或 旋转 和 矩阵 ,其 左上 角 不 再 标注 ,假设 点 5 在 坐 
标 系 1BI 下 位 置 不 变 , 则 点 S 的 速度 可 由 式 (4. 1 ) 两 边 对 时 间 微 分 得 到 , 即 

Voss =pporc+R’rs =vgorc+R’rs (4. 2) 

式 中 ,vs ,vgorc 分 别 表示 点 S AEIR B) 的 原点 在 坐标 系 14} 下 的 线 速度 。 这 里 要 先 解决 

旋转 矩阵 R 对 时 间 求 导 的 问题 。 假 定 ,刚体 坐 标 系 |B| 与 141 的 原点 重合 , 绕 原 点 做 旋转 运 

动 ,其 运动 完全 可 由 两 坐标 系 之 间 的 旋转 矩阵 R 表示 。 假 设 R 是 随时 间 变 化 的 光滑 函数 ， 
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矢量 "rs 通过 旋转 矩阵 光滑 上 映射 到 坐标 系 141 下 为 六 , 即 


r,-R(t)"r, (4. 3) 
在 任意 时 刻 ,存在 关系 式 
R(t)R'(t)=1 (4.4) 
式 中 ,I 表示 3x3 单位 矩阵 。 式 (4.4) 两 边 对 时 间 求 导 ,得 
R(i)R (CID)+R(CD)R (CD)=0 (4. 5) 
今 
N=R(t)R"(t) (4.6) 
进一步 可 以 得 
N+ -0 (4.7) 
显然 , 式 (4.7) 成 立 ,0 必然 是 一 个 反对 称 矩 阵 。 根 据 式 (4.6) RCOON 
R(t)= OR(t) (4. 8) 
式 (4.3) 两 侧 对 时 间 微 分 ,得 
r,-R(t)"r, (4.9) 
式 (4.8) 代 入 式 (4.9) ,得 
rs ORO)" r;- 0r, (4. 10) 


如 果 刚 体 绕 定 点 以 eo 角速度 旋转 运动 ,显然 ,刚体 上 S 点 的 速度 在 参考 坐标 系 |41 下 可 
以 表示 为 
rs = Xrs (4. 11) 
比较 式 (4. 10) 和 (4. 11) ,得 
Q — wx (4. 12) 


0 -oe. o, 
Q == w, 0 "(0 
-w, W, 0 


将 式 (4. 8) 代 入 式 (4.2) ,得 到 点 5 的 速度 ws 为 


依据 附录 A 式 (13) ,可 知 


Vs =V porc *€9XF s (4. 13) 
XX (4. 13) 两 侧 对 时 间 求 导 ,可 以 得 到 $ 点 的 加 速度 as 为 
As 2G yog; (XT +OX( Xr ) (4. 14) 


式 中 ,agporc 表 示 1B| 坐标 系 原点 的 加 速度 ( Origin Point Aacceleration) ;axrs 表示 转动 加 速度 
( Rotation Acceleration ) ;wx(wxrs ) 表示 问心 (离心 ) 加 速度 (Centripetal Acceleration) 。 式 
(4.6) 是 一 个 重要 的 结论 ,揭示 了 描述 刚体 运动 的 欧 拉 角 转动 角速度 和 刚体 实际 的 转动 角 
速度 的 映射 关系 ,刚体 转动 角速度 w 的 物理 意义 表示 在 瞬时 刚体 旋转 姿态 的 改变 ,可 以 看 
成 瞬时 绕 空 间 某 轴 线 e 以 角速度 0 做 旋转 运动 ,表示 为 


w, €x 
w=| w, | =0| e, | 26e (4. 15) 
C. €. 














因此 [w。 w, w.] 与 欧 拉 角 所 表示 的 绕 坐标 轴 三 次 有 序 转动 的 角速度 不 是 一 一 对 应 
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的 关系 。 参 考 式 (4. 8) ,得 到 以 下 关系 式 





0 o^ 0 
Q= 0 - -RR' (4. 16) 
2. ”0 0 
如 果 R 表示 为 
ra Fs Ty 
= r5 r4 








则 可 以 写 出 三 个 独立 的 方程 ,转动 角速度 各 分 量 可 以 表示 为 
Q, 三 731721 十 732722 tT33T23 
， a (4.17) 
O, S T4 Tj) HaTi 十 23713 
【 例 4.1】 考虑 一 个 基本 的 旋转 ,刚体 绕 x 轴 转 动 a 角 ,a 是 时 间 的 函数 , 则 旋转 矩阵 
表示 为 


1 0 0 
see ca(t) E 
0 salt)  ca(t) 
根据 式 (4.8) ,得 
0 0 0 L 0 UU 0 0 0 
a-i -le —sa E cat «Jer 0 s| 
0 a 0 


O cœ -sajl0 -sa ca 


l 
=o (4.18) 
0 


参考 式 (4. 15) , 式 (4. 18) 的 物理 意义 是 刚体 绕 x 轴 以 a 速度 旋转 。 

【 例 4.2】 如 果 以 RPY 角 描 述 在 参考 坐标 系 下 的 旋转 运动 , 绕 x 轴 \y 轴 和 z 轴 旋转 的 
三 个 角度 水 ,9 和 9 是 时 间 的 函数 ,其 描述 刚体 运动 的 角速度 为 
ý 
0 
e 

RPY 角 所 表示 的 旋转 矩阵 参考 式 (2. 28) ,利用 式 (4. 17) 可 以 得 到 刚体 实际 转动 角速度 
和 RPY 角 三 个 描述 转动 的 角度 随时 间 变 化 的 表示 形式 为 


进一步 得 


Opry = 











W, cocp -sp O][wv 
w, |=| côsp cp 0||ô (4.19) 
w, 一 SO 0 ] p 











位 写 为 
w= J rey Orry (4. 20) 
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A (4. 19) 重 新 写 为 























o, cocp -=s 0 
w, | ZV| cbsp |+0| cp |+9| 0|=we,+0e,+9e. (4.21) 
w, —sÜ 0 l 


显然 ,刚体 转动 角速度 o 是 瞬时 分 别 绕 e, e, 和 e, 轴 以 角速度 少 ,6 和 9 转动 的 合成 ， 
那么 单位 矢量 e,,e, 和 e, 如 何 得 到 呢 ? RPY 角 描 述 刚体 姿态 是 绕 参考 坐标 系 * 轴 \ 轴 和 := 
轴 的 三 次 有 序 转动 , 则 e.,e, Ae, 可 以 表示 为 


0 0 0 
e.=Rot(z,p)Rot(y,6)| 0|,e,=Rot(z,p)|1 ZI (4. 22) 
l 0 1 

















从 式 (4.22) 可 以 看 出 ,后 续 的 转动 改变 了 当前 转动 轴线 的 方向 ,例如 ,描述 刚体 旋转 运 
动 时 ,首先 绕 x 轴 [0 0 1] 转动 角速度 为 峭 ,因为 后 续 描 述 刚体 的 旋转 运动 又 绕 y 轴 和 
轴 分 别 转动 了 0 角 和 o 角 ,最 终 * 轴 的 方向 由 [0 0 1] 变 为 e., 最 后 绕 z 轴 的 转动 轴 方 
HREM. 


4.1.2. AAR TREE DEE 53 AGREE 2T 


如 图 4.2 所 示 , 点 C 相对 于 刚体 做 平移 运动 ,在 瞬时 ,点 C 在 两 个 坐标 系 下 14} 和 1B| 
的 位 置 矢量 关系 为 
Po-Psosz tK re (4. 23) 
由 式 (4. 23) 两边 对 时 间 微 分 得 到 C 点 的 速度 vc 为 
Vc =pe -pgos; R^ rz R"r, =Vpo0rc -QR"r, Rr, (4. 24) 


式 中 , 令 voa R'ro 表示 在 坐标 系 141}1 下 C 点 相对 于 刚体 的 速度 。 在 坐标 系 141 下 , 式 
(4. 24) 可 以 写 为 更 直观 的 形式 为 
Ve =V porc * V cre *€9XF c (4.25) 
式 中 ,omw 为 刚体 的 转动 角速度 。C AERAR IA 下 的 加 速度 ,可 通过 对 式 (4. 25) 两 边 对 时 
间 求 导 ,得 
=Pone+42R2r A QR"r, HR2S7r Rr, +R27 (4.26) 
将 式 (4.8) 代 入 式 (4. 26) ,得 
Ve =Vporc HOR’r. HI2R2r HOCDR2r HR27r +R2r = 
V porc * (42422) Rr. 2,0R*r 4 R^r, (4.27) 
可 以 写成 以 下 形式 
Ac d gogo tA o, *240XT Fa XT +OX( Xr ) (4. 28) 
式 中 ,a 表示 刚体 转动 角 加 速度 ;av - R^rz 表示 相对 加 速度 ( Relative Acceleration) ;2«xr, = 
202R?r。 表示 科 式 加 速度 ( Coriolis Acceleration) ,其 他 项 物理 意义 与 式 (4. 14) 一 致 。 
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图 4.2 点 C 相对 于 刚体 做 平移 运动 


4.2 串联 工业 机 械 臂 速度 分 析 


本 市 将 导出 机 械 臂 末端 线 速度 、 角 速度 和 机 器 人 关节 速率 之 间 的 关系 。 考 虑 机 械 臂 关 
节 空 间 有 nn 个 关节 ,末端 有 m 个 自由 度 , 其 关节 变量 为 9=[9， 0, … 9,] ,末端 坐标 系 
in+1| 在 基 上 坐标 系 11| 下 的 位 置 与 姿态 用 齐 次 矩阵 表示 为 
T= i (4.29) 
HHLA ZT, 0, RCO) 和 p(0) 都 是 时 间 的 函数 ,那么 末端 的 角速度 eo 和 线 速度 v 与 
关 市 速率 之 间 关 系 可 以 表示 为 
Q-R(0)R(0)' 


v=p(0) (4. 30) 
式 (4. 30 ) 经 过 整理 ,得 
w=] ð 
v=J,ĝ (4.31) 
可 以 统一 写 为 
HN -Jó (4.32) 
y 


K E 
eh J| $ | [o v]" 表示 一 个 刚体 的 速度 矢量 ,该 速度 包含 了 角速度 矢量 和 线 速度 和 
量 ,矩阵 了 称 为 雅 可 比 矩 阵 (Jacobian Matrix) ,为 瞬时 关节 速度 映射 为 未 端 速度 的 传递 矩阵 。 


4.2.1 微分 方法 


假设 经 过 推导 得 出 机 器 人 的 正 运动 学 为 
x-f(0),x c R",0cR' (4. 33) 
则 式 (4. 33) 对 时 间 求 微分 ,得 
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; FO); FO) 





"i ot 900, 6, 
(4. 34) 
X, M) ), ecu 0, 
以 上 公式 重新 写成 矩阵 的 形式 为 
afi) fiO) 
06, 00, 
x- : : Jð (4.35) 
9f,(0) ^  af,(0) 
00, 00, 
末端 速度 与 关节 速度 的 关系 可 以 简写 为 
x-J,0,J e R” (4. 36) 


式 中 ,矩阵 J, 称 为 分 析 雅 可 比 和 矩阵 (Analysis Jacobian Matrix) 4. WA SX (4. 33) x = 
[p Pl 表示 末端 的 位 置 和 姿态 欧 拉 角 ,p 为 从 基 坐 标 原点 指向 末端 坐标 系 原 点 的 矢量 ,四 
表示 末端 坐标 系 相 对 于 基 坐 标 系 的 姿态 欧 拉 角 ,例如 四 = [水 ,9,p] 为 RPY 表示 方式 的 三 
个 欧 拉 角 。 则 末端 的 速度 [w v] 与 x 的 关系 为 


Lo j BA (4. 37) 
根据 式 (4. 37) 也 可 得 
I 0 
Là | o m | (4. 38) 


【 例 4.3】 平面 2R 机 构 雅 可 比 矩 阵 图 4.3 描述 了 平面 2R 机 构 , 建 立 D-H 坐标 , 连 
IKEK L M D REEL (x, ye) S2& B AERE CÓ, ,0 ) 的 正 运动 学 方程 写 为 





图 4.3 平面 2R 机 构 


PA they 


(4. 39) 
y, ls; ls; 
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式 (4.39) 对 时 间 求 微分 ,并 写成 矩阵 的 形式 为 
[755 *h555) hse] . 
ge 0 
| he 6, i] 
式 中 ,x=[x。 y,]1',0-[6, 6,1". r9 SI RS TE RT HEAR EOS 
-(hsi hs) 一 /si 
a] lici 6, Yig 
如 果 把 式 (4. 41) 表 示 成 列 矢量 的 形式 ,有 
J-|J, J. | 
也 可 写成 
x -J,0, 4J,6, 


(4. 40) 


(4.41) 


(4. 42) 


Hg] 4. 3 表示 出 了 J 和 J 在 平面 的 方向 ,J, RIRE EGÓB PERUSRS — 90 , 3E ECT T 2.J, 


为 雅 可 比 矩 阵 的 第 一 列 , 但 不 垂直 于 杆 1 ,而 是 垂直 于 00,。 显 然 ,关节 2 转动 速度 ò, 通过 
万 映射 到 末端 ,方向 为 /规范 化 后 的 单位 矢量 ,速度 大 小 为 1J,16, =1.6, , 同 理 , 关 节 1 转动 


速度 0, 通过 映射 到 末端 ,末端 速度 是 两 个 关节 映射 速度 的 释 加 。 


【 例 4.4】 空间 RRP 球 型 机 构 雅 可 比 和 矩阵 ”各 几何 参数 如 图 4.4 所 示 ,未 端的 位 置 矢 


量 为 p=[p。 p, p.] ,关节 变量 为 6=[90， 6, d] ,推导 其 正 运动 学 方程 为 


p, 7 d5€,6, 
p, 7 d4658, 
p, 7d, *d,s, 
两 边 对 时 间 求 导 ,得 
—d.6,5,  —d4s,€, GC 
D —d55,5, ^a 
0 d. 6» S, 


式 中 ,v 为 未 端的 线 速 度 ;6 为 关节 速率 ,0=[6 6, d,]". 





图 4.4 空间 RRP 机 构 


4.2.2 几何 方法 


如 图 4. 5 所 示 , 关 节 i 和 关节 j 分 别 为 R 型 和 了 型 关节 ,关节 ;的 转动 角 速 率 为 和 , 则 在 
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空间 的 转动 速度 为 6.e:( 其 中 e; 为 转动 关节 轴线 单位 方向 ) ,关节 的 移动 速率 为 dj, 则 其 空 
间 的 平移 速度 为 djej,e; 为 移动 关节 轴线 单位 方向 。 当 关节 i 或 关节 j 单独 运动 时 ,其 余 关 
节 锁 定 ,其 关节 速度 对 机 械 臂 末 问 线 速度 和 角速度 的 映射 见 表 4. 1, RP r 表示 从 关节 轴线 


指向 末端 操作 点 的 天 量 。 
表 4.1 关节 速度 对 末端 速度 的 影响 


末端 转动 关节 移动 关节 
线束 度 beor, ia 
角速度 ó,e; JG 


T a 
— — 
— 
— 





n+l} 


图 4.5 关 市 速度 到 末端 速度 的 映射 
显然 ,串联 机 器 人 末端 的 速度 是 各 运动 天 市 速度 通过 表 4. 1 所 示 的 方式 映射 到 末端 并 
琵 加 得 到 的 。 在 机 如 人 基 坐 标 系 11| 下 ,首先 分 析 机 械 臂 前 、 后 两 个 连 杆 的 角速度 和 线 速度 
EKR KT i 为 转动 关节 ,所 建立 的 坐标 系 .关节 的 轴线 方向 如 图 4.6(a) 所 示 , 假设 
FF i-1 的 角速度 为 w; | , 则 杆 i 的 角速度 可 以 表示 为 
w; =w; +0.0,,i=1,2,°,N (4. 43a) 
旋转 运动 杆 件 上 每 一 点 的 角速度 相同 ,通常 情况 下 ,每 一 点 的 线 速 度 不 同 ,为 了 描述 杆 
的 线 速度 ,需要 选择 一 个 点 ,以 此 点 的 线 速度 作为 杆 件 线 速度 的 表示 ,在 本 书 中 ,此 点 位 于 与 
杆 件 固 连 坐标 系 的 原点 处 ,如 对 于 第 i 杆 件 ,坐标 系 ,| 固 连 于 此 杆 件 ,其 原点 0 为 运动 描 
述 点 ,用 此 点 速度 表示 杆 的 线 速 度 。 下 面 分 析 杆 件 i 运 动 描 述 点 0;,, 处 的 线 速度 ,0, 为 杆 i- 
1 与 杆 i 的 重合 点 ,在 其 位 置 建立 一 个 瞬时 坐标 系 F'. ,此 坐标 系 方向 与 F; 坐标 系 一 致 , 固 连 
于 杆 i 上 ,如 图 4.6(b) 所 示 , 显 然 , 此 坐标 系 原点 处 的 速度 也 为 v;, | ,v; | 表示 i-1 杆 在 0; 点 
处 的 线 速 度 ,参考 图 4. 6(b) 和 式 (4. 13) ,可 知 
V =P ta ild (4. 43b) 
如 果 第 i 关节 为 移动 关节 ,所 连接 的 杆 件 i-1 和 杆 件 i 之 间 的 关系 如 图 4.7 所 示 ,因为 
移动 关节 不 允许 任何 的 相对 转动 , 杆 i 和 i-1 有 相同 的 角速度 ,连接 F, A Fa 坐标 系 原点 的 
初始 矢量 ao 为 常量 , 沿 着 关节 轴线 方向 移动 位 移 为 di。 并 参考 图 4.7 和 式 (4. 25) ,可 以 得 
出 两 杆 之 间 角 速度 和 线 速度 之 间 的 递 推 关 系 式 为 
€; — €9; 1 
f mg. . +d jie;+CO 1X(ant+d.e,)=»,) +d €+; Xa, (4.44) 


式 中 ,ai=ao+diei 为 关节 i 从 初始 位 置 运 动 后 0; 点 到 0;, 点 的 位 置 矢量 。 
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图 4.7 了 型 关节 速度 递 推 
【 例 4.5】 已 知 空间 2R 机 构 , 某 时 刻 关节 位 置 为 9, ,9, ,速度 大 小 为 9, , ,空间 各 几何 
参数 如 图 4. 8 所 示 ,0,,0, ,0 为 所 建立 坐标 系 的 原点 ,其 中 0, 为 基 坐 标 系 原 点 ,假设 瞬时 
关节 轴线 方 问 为 el ,e, ,坐标 原点 之 间 的 位 置 天 量 为 e ,a, , 试 推 导 此 时 末 闯 的 速度 。 杆 1 的 
角速度 与 线 速度 表示 为 
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w, 70, *0,€, 

y, =V t€), XA, (4. 45) 
XP oy 2v, 20 为 第 0 杆 , 即 基 座 的 速度 ,w 为 与 杆 1 固 连坐 标 系 F, 原点 相对 于 F 的 位 置 
矢量 。 杆 件 2 的 角速度 与 线 速 度 为 

w, =w; *0,€, 

V, =V, +0, XA, (4.46) 

如 果 以 eo 和 v 表示 末端 角速度 和 线 速 度 ,将 式 (4.45) 代 入 式 (4.46) ,得 
(7 (9; -0,e, *0,e, 
y -V,-(9,X4, +0, Xa, (4. 47) 

对 式 (4. 47) 进 一 步 整 理 , 得 

Q9 -0,e,*0,e, 


y -0,e,x(a,«a,) +0,e, xa, 


人 [加 = 


因此 ,在 基 坐 标 系 {11 下 得 到 了 末端 速度 和 关 闻 速度 的 关系 。 和 矩阵 了 是 在 基 坐 标 系 下 
得 出 的 雅 可 比 和 矩阵 ,矩阵 了 各 项 所 包含 的 各 关节 轴 单 位 方 问 e; 和 各 坐标 系 原点 的 相对 位 置 
矢量 a; 依赖 于 所 在 的 坐标 系 。 


写成 矩阵 形式 为 








图 4.8 空间 RR 机 构 


【 例 4.6】 如 图 4.9 所 示 , 已 知 空间 RP 机 构 , 某 时 刻 关 节 位 置 为 9, 和 d, ,速度 大 小 为 
0, Il d , ,推导 此 时 末端 的 速度 。 杆 件 1 角速度 与 线 速度 为 


w = =0w0+01e) 


y, =V +W; Xd, (4. 48) 
AF, w 7», 20;a, 为 与 杆 1 固 连坐 标 系 FE, 原点 相对 于 坐标 系 F, 的 位 置 天 量 。 杆 件 2 的 
角速度 与 线 速 度 为 
h =W; 
v, =V, +d e, +0, XA, (4.49) 


如 果 以 w 和 v 表示 末端 角速度 和 线 速 度 PESCA. 48) ARAI CA. 49) ,得 


c — 0, e, 
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v 20,e, x(a,*a,) *d,e, 


| 01 [ 6, 6, 
MERE M 


写成 矩阵 形式 为 





- 


A 


图 4.9 空间 RP 机 构 
假定 关节 ,nm+l 个 杆 件 的 机 械 臂 如 图 4. 10 所 示 , 各 关节 位 置 以 0; 表示 ,速度 大 小 以 À; 
表示 ,关节 轴线 单位 方向 以 e; 表示 ,坐标 系 F: 固 连 于 第 i-1 杆 件 没 有 画 出 , 仅 以 原点 0; X 
示 , 从 图 中 可 以 知道 各 杆 件 的 角速度 有 以 下 关系 : 


€, —0,e, 
e =0 € +ô €, 
S WS (4. 50) 
w, 70,6, 40,6, 4---40,e, 
TR ZR 9i HJ £6 358 8E w 表示 , 则 
w -6,e, + Ó,e, +: * Ó,e, = Y ĝe, (4. 51) 
1 
在 基 坐 标 系 |1| 下 ,末端 位 置 失 量 p 可 以 表示 为 
p=a,ta;,+*…+a, (4. 52) 
式 (4. 52) 两 侧 对 时 间 求 导 , 得 
p-à,*à,*---44, (4. 53) 
式 中 ,参考 式 (4. 11) ,ai =wixa; ,进一步 ,将 式 (4. 50) 代 入 式 (4. 53) ,得 
p -6,e,xa, * (0,e, 40,0, ) xa, ---*(0,e, 40,6, 440,0, )xa， (4. 54) 
重新 整理 为 
p -0,e,x(a,*a,4---«a,) 40,e,x(a,*a,4--a,) 4--40,e, xa, (4. 55) 
4 r, EON 
r,-d;t*d; t4, (4. 56) 


表示 在 基 坐 标 系 下 ,从 0; 指向 末端 操作 点 。 如 果 令 末端 线 速 度 为 v=p , 式 (4. 55 ) 重新 写 为 


一 2, 6e; XT, (4. 57) 
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d, i 2 
e NX 
第 1 杆 件 en A 
= NS 
es 
4A o, fA ort 
图 4. 10 nn 关节 串联 机 械 辟 
式 (4. 51) 和 式 (4. 57) 写成 矩阵 的 形式 可 以 表示 为 
ô, 
e e ... e, 0 
N -| | i à (4. 58) 
y EXE Uo eX. : 
Ô, 


得 出 末端 速度 [w v] 与 关节 速率 矢量 9 关系 ,简写 为 
t=J0,JeR™ (4. 59) 
式 中 ,ti=[w v]',0-[0, 6, -- 0,]'。 这 里 ,J 是 机 器 人 机 构 的 雅 可 比 矩 阵 ,可 以 写 为 


J, 

7=|7 (4. 60) 

式 中 
J =le, e = en 
T= &xrn = eom] 
S J 表示 和 雅 可 比 和 矩阵 J 的 第 i 列 , 则 

€; 

hs 


TIR “关节 是 移动 关节 ,参考 式 (4. 44) ,此 时 
a, -d,e «c, ,xa, 

在 这 种 情况 下 

mw=Obiei+De+…+D ,0, i+O e+…+Oe 

y 20, e, xr, 40,6, xr, 4-40, ,e, xr, +de+…Oe xa, 

关节 矢量 0 定义 为 

0 - | ò, à; … Å., d, Ms 0 ]” 
ERE J B i IEH 
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对 于 6R 机 械 臂 ,JeR” ,在 矩阵 非 奇 异 前 提 下 ,可 以 通过 解 方程 得 出 0 为 


ó-J^ ^] (4. 61) 


【 例 4.7】 推导 空间 关节 6R 机 械 臂 雅 可 比 和 矩阵 ,如 图 3. 13 所 示 。 根 据 式 (4. 58 ) , 首 
先 求 导出 e, - es 以 及 rl ~rs, AIHH D-H 方法 ,详细 过 程 如 下 : 
关节 各 轴 的 单位 矢量 为 





| cls23 

e, = ud = ri =| $1353 
一 C1C2334 十 S1C4 

-Q, mm: a " |- 7841€5384 —7C,€, 
782334 


e, - Q, oowoo o = = 0 o 


(51€25€4 —€,54 ) S tints 





PH ( €,€53€4 5,54 ) 35 十 C1823C5 
$23C455 7€23€5 
A 


推导 位 置 天 量 产 ,首先 根据 图 3. 13 ,在 e, B2 


至 得 到 的 前 提 下 ,手腕 中 心 点 指向 末端 的 矢 
量 为 


—— 
0,0; -p; -p. - dge; 
c, (a4 *a4c, *a404, 455, ) 
——À 
ri -p;-pi-pc-pi*0,0; =| s (aí*a,0,*a,05*d,55) | d.e, 
0555 *44$5, 045, 
c, (450, ta404, *d,55,) 
r,-p,-p,-| s, (25058405 *d,5,,) | *d,e, 
05$, 0435, -d 473 
c, (a4, *d,5,,) 
r, =P1-P3 =| s, (0505 d455) | *d,e, 
0485 dacs 
C353, 
r,-p;-p,-| 5155, | *d,e, 


-d, C53 
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r;-r,;-p;,-p,-d.e, 
UB p; RIERA il 原点 在 坐标 系 11} 下 的 位 置 矢量 。 根 据 式 (4. 58) , 则 可 以 推导 出 6R 
机 械 臂 的 雅 可 比 和 矩阵 。 


4.2.3 雅 可 比 矩 阵 的 变换 
在 坐标 系 |1| 下 ,通过 以 上 推导 可 知 , 当 第 i 关节 为 转动 关节 时 , 雅 可 比 矩 阵 第 i 列 为 


€; 
a Pn 
e;Xr, 


当 第 i 关节 为 移动 关节 时 , 雅 可 比 矩 阵 第 i 列 为 


|, 


显然 , 雅 可 比 和 矩阵 的 列 矢量 依赖 于 所 在 的 坐标 系 ,如 果 *e,,“r, 表示 在 第 个 坐标 系 下 ， 
MIJET HERE J 是 在 第 个 坐标 系 下 建立 的 。e;,'r; 表示 在 第 i 个 坐标 系 下 的 矢量 ,变化 到 
坐标 系 F, 下 为 
0 
0 
] 
“p=°R,'r. (4. 63) 
式 中 ,“R; 表示 第 i 坐标 系 相对 第 坐标 系 的 旋转 矩阵 。 在 第 i 个 坐标 下 建立 的 雅 可 比 矩 阵 
要 变换 到 第 个 坐标 系 下 ,可 以 通过 式 (4.62) (4. 63) f8 
R 0] 
| ' | IE (4. 64) 
0 "R, 
如 图 4. 11 所 示 ,假定 在 坐标 系 |1| 下 ,已 经 建立 末端 速度 与 关节 速率 矢量 之 间 的 雅 可 
HEEREN 


*e;="R;'e;="R, (4. 62) 








|=" (4. 65) 


AD RAKGRCT RE 5 5 /.— 4-28 IAE DLE HE, FRAR E ERRAI) FSK 
端 操作 点 0 位 置 关系 为 OF =ro W E SKAREN 





图 4.11 雅 可 比 矩 阵 在 不 同 坐 标 系 下 的 表示 


yg VOX; 


写成 矩阵 形式 为 
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M d | 四 (4. 66) 
式 中 ,S(rio) 表 示 -roz 构 造 的 反对 称 矩 阵 。 则 已 点 的 速度 与 关节 速率 矢量 之 间 的 关系 为 
[.] "sco j| à (4.67) 


在 F, 坐标 系 下 ,E 点 的 角速度 和 线 速度 与 关节 速率 矢量 0 之 间 的 关系 ,可 通过 以 下 式 
得 到 : 


| E |. ace m 中 有 
式 中 ,R, 表示 坐标 系 F, 相对 于 坐标 系 F, 下 的 姿态 矩阵 。 
4.2.4 ”关节 速率 的 求解 


在 通常 情况 下 ,矩阵 不 能 通过 符号 求 道 ,b 的 计算 通过 数值 方法 ,适合 的 方法 有 高 斯 消 
元 法 及 LU 分 解法 。6R 解 耦 型 机 械 臂 可 以 用 简单 的 方法 求 6, 由 式 (3.31) 可知, 两 边 对 时 间 
求 导 ,得 




















0 0 0 
v=pc+Q| 0 |=v.+00| 0 | =w+wxO@| 0 | Zv.*cx(p-p.) (4. 69) 
d. d, d, 
vc 可 以 表示 为 
y, -v-«ox(p-p.) (4. 70) 


手 胶 中 心 点 在 最 后 三 个 关 市 轴线 交点 上 , 线 速度 不 受 后 三 个 关节 运动 的 影响 。v。 可 以 
写成 


y, =0,e, xr, *0,e, xr, *0,e, xr, (4. 71) 
在 这 种 情况 下 ,矢量 r 定义 为 从 0, 点 指向 C 点 , 另 一 方面 
w=0 e, +0,€, + +0525 (4. 72) 
MFP GJER EERE EXA 
Jè di å 
MOM HC (4. 73) 
A AR Ld AE OI 3I e] T , 则 
Juzle..e; 6€] 
Jae C 4] (4. 74) 
Tle xr, 43595 499] 
关节 速度 矢量 0 分 解 为 
0. 
| 
0, 


= 
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Å, 0, 
2 | ZH 
Ó, 06 
式 (4.73) 可 以 进一步 写成 
op MEN (4. 75) 
J,,0, 2 -J,,0. 
从 式 (4.75) 可 以 看 出 ,首先 要 解 出 0. ,然后 再 解 出 9. , 则 可 以 表示 为 
B. Jv. 
(4. 76) 
0,-J15(«-J,,0.) 
AH Ja Jo AHAN 
det(J5, )= (e,xr, ) xCe;xr,) * Cexr,) 
det(J,,)- e,xe; * e, 
meni 
P l 
E A EEFT ES [Co ers yd Br 1 
det( J ) 
i [Ca xr, Je )] 


(esxe5) 
Y ] T 
nig] n | 


(e,xe;) i 
4.2.5 奇异 位 形 分 析 


如 果 已 知 末 端 速度 ,计算 关节 速率 9 时 , 雅 可 比 和 矩阵 J 不可逆 (或 者 称 为 奇异 ) 时 , 则 无 
法 计算 。 此 时 ,所 对 应 着 的 机 龙 人 的 位 形 称 为 奇异 位 形 , 当 机 器 人 为 非 匈 余 驱 动 时 , 雅 可 比 
ABE J 满足 

det(J)=0 (4.78) 
时 ,机 絮 人 必然 处 于 奇异 位 形 。 

在 满足 此 条 件 下 ,瞬时 存在 非 零 的 关节 速率 矢量 90, 而 机 器 人 末端 速度 在 某 一 方面 输出 
为 零 , 即 无 限 小 的 运动 在 末端 某 一 方 癌 无 法 完成 或 者 说 机 器 人 末端 此 时 失去 一 个 或 者 多 个 
目 由 度 。 

【 例 4.8】 分 析 平 面 2R 机 械 臂 奇异 位 形 , 参 考 式 (4. 41) , 雅 可 比 和 矩阵 的 行列 式 的 值 为 

det(J)= Ll, L,sin 0, 

^ det(J)=0, 则 0, ZO , m , POSEE BBUP (VE A E] 4. 12 所 示 。 

当 0,20, ET, TERT ELARIEERUA AR TE J,-— J ] 方 向 平行 ,机 械 辟 末端 瞬时 沿 此 方向 的 垂 
直方 向 无 法 移动 一 个 极 小 位 移 ,此 时 2R 机 械 臂 自由 度 由 2 降 为 1。 

在 解 兢 型 机 器 人 关节 速率 计算 时 ,已 经 假设 Jo Ja EAF. WR J 奇异 , 则 关节 速率 
0 无 法 计算 ,而 .J 奇 异 ,J2 非 奇异 时 ,可 以 计算 出 前 三 个 关节 的 速率 。 首 先 分 析 Ja , 它 决定 
了 关节 速率 9 是否 可 能 解 出 ,J 和 矩阵 包 含 三 个 列 矢量 exr e; xr, A ex ,因此 三 个 矢量 共 
面 或 其 中 两 个 矢量 平行 ,或 至 少 有 一 个 矢量 为 零 , 则 J, 奇异。 一 般 解 而 型 机 器 人 两 个 连续 
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图 4.12 平面 2R 机 构 奇异 位 形 

的 轴 垂 下 或 者 平行 ,如 图 4. 130a) Brzs ,假设 第 1,2 MEH, FPO C 点 位 于 两 轴 相 交 的 
平面 m EC 点 恰好 位 于 矢量 线 e 上 ,此 时 eixri=0, 否 则 (ejxri)/W (esxr,) ,det(J)= 
0。 显 然 , 瞬 时 前 两 轴 的 运动 只 能 使 C 点 在 垂直 rz 平面 方向 运动 ,此 时 无 法 在 ri 平面 内 且 
治 与 es xm 垂直 方向 运动 ,此 种 奇异 称 为 肩 部 奇异 (Shoulder Singularity) 。 如 果 第 2 与 第 3 
轴 平 行 ,C 点 位 于 两 条 轴线 所 在 的 平面 ra 上 ,此 时 (exrm ) / (esxrm ) ,det(J,,)2 0,C 点 瞬 
时 无 法 在 rz 平面 上 且 回 与 elxm EED a , ,此 时 奇异 位 形 称 为 肘 部 奇异 位 形 (Elbow 
Singularity ) 。 

当 ,奇异 不 可 逆 时 ,手腕 处 于 奇异 位 形 , 手 腕 三 条 转动 轴线 共 面 时 ,有 

det(Ji,)2 e,xe, - e, -0 (4. 79) 
可 以 想象 , 当 轴 4 和 轴 6 共 线 时 ,满足 式 (4. 79) 的 条 件 ,如 图 4. 13( e) Brzn e 





(b) 肘 部 奇异 位 形 
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(c) 球形 手腕 奇异 位 形 
图 4.13 6R 型 机 械 臂 奇异 位 形 


4.2.6 ”机 如 人 性 能 指标 


对 于 瞬时 确定 的 位 形 9,t=J6 定义 了 一 个 线性 系统 ,可 以 认为 了 是 输入 0 产生 输出 上 的 
比例 影射 ,定量 分 析 此 比例 影射 是 一 件 有 意义 的 工作 。 雅 可 比 矩 阵 把 交 维 关节 空间 速率 影 
WE n 维 任 务 空间 末端 速度 ,在 一 般 情 况 下 ,t 前 三 个 元 素 的 单位 为 rad/s, 后 三 个 元 素 的 单 
位 为 m/s。 关 节 速 率 矢量 0 的 单位 依赖 关节 的 类 型 ,不 必要 有 相同 的 单位 。 如 果 所 有 关节 
是 转动 关节 , W) J 矩阵 前 三 行 是 无 量 纲 的 ,后 三 行 是 长 度 。 对 矩阵 J 进行 奇异 值 分 解 为 


J-U V =UV" (4. 80) 


On 


式 中 ,esR”,YeR” 是 正 交 矩阵 (旋转 矩阵 ) ,Ze 有 R 。e; 是 雅 可 比 矩 阵 J 的 奇异 值 。 
围绕 J 进行 的 映射 可 以 表达 为 一 个 n 维 的 椭 球 映射 ,向 量 在 映射 后 与 映射 前 的 模 长 之 比 称 
为 映射 的 放大 倍数 。y 由 大 到 小 的 个 奇异 值 0120; mo,20 为 映射 椭 球 的 六 个 主轴 
半径 ,或 称 为 沿 椭 球 n 个 主轴 方向 的 映射 放大 倍数 。 

1. 条 件数 指标 

如 条 了 是 奇异 的 , 则 椭圆 球 至 少 一 个 半 轴 退化 为 零 , 另 一 方面 如 果 和 矩阵 了 是 各 向 同性 
(Isotropic) 的 , 则 奇异 值 相等 , 它 的 影射 是 从 一 个 单位 球 影射 成 男 一 个 放大 或 缩小 的 球 。 上 串 
联机 大 人 的 雅 可 比 矩 阵 , 可 将 关 市 速度 空间 的 单位 球 通过 旋转 、 比 例 放 大 、 旋 转变 换 后 影射 
到 任务 空间 笛 卡 儿 速 度 的 一 个 椭圆 球 ,如 果 雅 可 比 和 矩阵 的 各 个 元 素 具 有 相同 的 单位 , 则 可 以 
定义 条 件数 cond(J) 为 J 了 的 最 大 值 o, 与 最 小 值 o, 的 比 , 即 


cond(J) - = (4. 81) 


该 指标 也 称 各 回 同 性 指标 ,强调 治 各 方向 的 映射 放大 倍数 不 能 差距 太 大 。 因 为 or, 为 映 
射 的 最 大 放大 倍数 ,or 为 映射 的 最 小 放大 倍数 ,二 者 的 比值 越 接 近 于 1 ,表示 沿 各 方向 进行 
的 映射 放大 倍数 越 接近 。 当 比值 为 1 时 , 沿 各 方向 进行 的 映射 的 放大 倍数 完全 相等 ,这 种 情 
况 称 为 各 加 同性 。 另 一 方面 , 当 雅 可 比 矩 阵 最 小 奇异 值 为 零 时 ,条 件数 无 穷 大 ,可 以 看 成 从 
关 世 空间 单位 球 映 射 到 任务 空间 失真 ,此 时 椭圆 球 一 个 半 轴 退化 为 零 ,在 三 维 空间 中 ,椭圆 
球 退 化 为 一 个 椭圆 。 在 应 用 以 上 指标 时 应 注意 :如 果 串 联机 器 人 各 关节 为 转动 关节 ,9 对 oo 
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等 与 旋转 有 关 物 理 量 的 映射 ,由 于 映射 前 后 量 纲 不 发 生变 化 ,因此 是 纯粹 的 放大 倍数 关系 ; 
而 0 对 v 线 速度 物理 量 的 映射 ,在 映射 后 量 纲 将 发 生变 化 ,因此 并 不 是 纯粹 的 放大 倍数 关 
系 ,而 是 一 种 带 量 纲 的 放大 倍数 。 因 此 把 8 向 v 向量 进行 映射 时 就 会 产生 一 些 问题 , 比如 ， 
当 把 机 构 按 比例 缩放 时 ,或 者 仅仅 由 于 使 用 的 长 度 单位 不 同时 ,各 种 性 能 指标 就 会 发 生 很 大 
变化 。 这 是 由 于 一 方面 映射 的 放大 倍数 会 随 着 量 纲 的 改变 而 改变 ;为 一 方面 ,与 量 纲 无 关 量 
同 与 量 纲 有 关 量 在 整体 映射 向 量 中 的 相对 比重 也 会 发 生 很 大 变化 。 总 体 而 言 ETE RI EAR 
阵 中 元 素 单位 不 同 , 则 条 件数 的 定义 不 能 应 用 ,因为 单位 不 同 , 无 法 将 奇异 值 从 大 到 小 排列 ， 
这 就 给 对 机 构 的 性 能 的 正确 评价 带 来 困难 ,为 了 解决 这 个 矛盾 ,需要 定义 一 个 特征 长 度 比例 
因子 ,用 来 处 理 具 有 长 度 单位 的 雅 可 比 矩 阵 元 素 ,产生 新 的 无 量 纲 雅 可 比 和 矩阵 J。 

2. 可 操作 性 指标 

在 经 典 控制 理论 中 ,经常 分 析 一 个 单 输入 . 单 输出 称 为 脉冲 啊 应 系统 ,通过 单位 输入 分 
析 系 统 的 性 质 。 在 多 维系 统 里 ,基本 与 之 类 似 , 但 输入 是 系统 输入 变量 的 单位 范 数 , 考 虑 串 
联机 器 人 关节 速率 矢量 范 数 为 | 


| 81? 260214848 <1 (4. 82) 
Ti RE n] HERE J IER, M 
0-J^t 
LOI =O= (J^) (J^ 0- € (JJ) t (4. 83) 


XX (4. 82) 定 义 了 一 个 n 维 椭圆 球 , 称 为 可 操作 性 椭圆 球 。 将 式 (4. 80) 代 入 式 (4. 83) , 

得 
JJ) t-(U't)' x ^(U't) (4. 84) 
式 中 


wXxw-Y-«Lisl2a (4. 85) 

式 中 ,w; 为 矢量 w 中 的 项 , |wi = I UE = tl ,如 果 输 入 (关节 速率 ) 的 范 数 矢量 是 单位 

范 数 ,那么 输出 (末端 速度 ) 将 位 于 式 (4. 83)J7 给 定 的 椭圆 球 之 内 。 可 操作 性 度量 指标 可 

以 定义 为 

Idet(J) | 2 / det(JJ^) = 0,070, (4. 86) 

该 指标 也 称 映 射 体积 指标 ,因为 0,0; 7-0, 为 映射 椭 球 于 个 主轴 半径 之 积 ,是 与 映射 椭 
球体 积 相关 的 量 ,强调 了 各 方向 映射 的 整体 特性 。 

条 件数 指标 和 可 操作 性 指标 均 可 用 于 对 机 构 的 整体 综合 性 能 进行 评价 ,二 者 基本 上 是 

一 致 的 。 目 前 比较 流行 的 是 采用 各 向 同性 指标 作为 串联 机 器 人 整体 综合 性 能 评价 的 主要 指 

标 ,但 这 项 指标 比较 明显 的 缺陷 是 :有 时 尽管 o. 与 os, 都 增 大 ,但 只 要 o, 的 增幅 超过 o, 的 

增幅 , 便 得 出 机 构 性 能 变 好 的 结论 ,这 显然 不 太 合理 。 而 可 操作 性 指标 则 将 映射 的 体积 作为 
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考察 对 象 ,更 适宜 用 于 机 构 整 体 性 能 的 评价 。 还 有 一 个 原因 在 于 映射 椭 球 的 最 长 轴 或 最 短 
轴 是 经 常 切换 的 ,因此 o 与 o, 之 比 连续 但 不 光滑 , 当 采 用 条 件数 指标 作为 优化 目标 函数 
时 ,有 可 能 会 陷 人 局 部 最 优点 ;而 可 操作 性 指标 则 是 连续 光滑 的 。 因 此 ,可 将 无 量 纲 雅 可 比 
和 矩阵 的 行列 式 值 1det(J* ) 1 作为 机 絮 人 性 能 评价 的 主要 指标 ,该 值 越 大 ,表示 机 器 人 的 整体 
性 能 越 好 。 
【 例 4.9】 考虑 平面 2R 机 械 臂 ,如 图 4. 3 所 示 , 分 析 在 不 同位 形 下 的 条 件数 指标 。 令 
几何 参数 为 上 2/2. m,l,=1 mm, 则 其 雅 可 比 矩 阵 为 
pua iN 
J= 
42 c, 6j C12 
当 0, 20,0, 2m/4 时 ,关节 速率 单位 矢量 圆 映射 到 任务 空间 末端 速度 ,如 图 4. 14(a) 所 
示 , 此 时 
2.414 2 0 
5-| 0 nl 
条 件数 cond(J)= 5. 824, 34 0, 20,0, -3a/4 时 ,此 时 


| 1 


条 件数 cond(J)- 1, ?4 8, 20,0, ZO 时 ,此 时 
2.6131 0 
z=] 0 i 
关节 速率 单位 矢量 圆 映 射 到 任务 空间 末端 速度 如 图 4. 14 ( b) 所 示 , 显然 , 当 0, 20 时 ， 
雅 可 比 和 矩阵 奇异 ,任务 空间 速度 映射 退化 为 一 条 直线 ,条 件数 cond (J) o 。 





图 4. 14 速度 映射 图 
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本 章 小 结 


本 章 首 先 从 刚体 一 般 的 速度 分 析 入 手 , 分 析 了 刚体 上 点 速度 、 加 速度 以 及 相对 刚体 平移 
速度 、 加 速度 的 表达 形式 ,讨论 了 串联 工业 机 各 人 雅 可 比 矩 阵 ,重点 人 研究 了 利用 微分 方法 及 
几何 方法 求解 雅 可 比 矩 阵 , 并 基于 雅 可 比 矩 阵 分 析 了 机 器 人 奇异 位 形 以 及 运动 性 能 评价 指 
标 。 


tB 5 xm — 刚体 一 般 运 动 及 螺 旗 


在 笛 卡 儿 坐 标 空间 里 ,刚体 瞬时 最 一 般 的 运动 形式 为 螺旋 运动 , 即 瞬时 刚体 存在 绕 着 空 
间 某 轴线 的 转动 与 沿 此 轴线 方向 的 移动 , 即 刚体 旋转 又 平移 ,刚体 的 纯 转 动 和 纯 移 动 只 是 蝶 
旋 运 动 的 特殊 情况 。 其 瞬时 运动 形态 可 等 效 于 如 图 5. 1 所 示 的 圆柱 螺旋 线 运动 ,刚体 转动 
并 沿 螺旋 轴线 方向 平移 ,假定 此 时 刚体 的 角速度 为 wm, 螺旋 轴 单 位 矢量 为 e, 刚 体 平移 速度 ， 
显然 与 角速度 w 及 螺 距 大 小 有 关 ,两 者 为 依赖 的 关系 ,从 螺旋 轴 指 加 点 4 的 矢量 为 r, 则 此 
时 刚体 上 点 4 的 速度 可 以 表示 为 [w (wxr+y)] 。 





图 5.1 刚体 瞬时 等 效 螺 旋 运 动 


5.1 空间 直线 的 Plücker 坐标 


空间 刚体 的 一 般 运 动 形式 为 螺旋 运动 ,螺旋 轴线 在 空间 方向 及 位 置 的 表示 是 首先 要 解 
决 的 问题 。 其 表示 可 采用 Plücker 坐标 表示 , Plücker 第 一 次 提出 在 三 维 空间 任意 一 条 直线 
的 坐标 表达 形式 。 假 定 空间 两 个 点 PURI P, , 按 一 定 的 顺序 连接 ,就 决定 了 空间 内 一 条 直线 
L 的 方向 和 位 置 ,两 点 的 空间 位 置 矢量 分 别 为 r,/ 和 7,, 如 图 5.2 所 示 。 如 果 有 一 点 P 位 于 直 
线 上 ,其 位 置 矢量 为 r, 则 与 点 P, 和 P, 关 系 为 

(rı-r,)x(r-r;)=0 9. L} 
XC. 1 ) 展开 为 
(r,-r,)xr*r,x(r,—r,)-0 
显然 PRA AE r-r, 和 rix(r,-r ) 完 全 定义 了 直线 ,表示 直线 在 空间 的 方向 和 位 置 ， 
定义 一 个 六 维 列 矩 阵 包 含 两 个 矢量 , 即 [7,-r， rix(r;-r)] ,和 矩阵 中 六 个 标量 元 素 是 直线 
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L 的 Plücker 坐标 ,进而 令 
ul ,n-r,xe (5.2) 
重新 得 到 的 列 和 矩阵 [e n] 是 直线 工 的 规范 Plücker 坐标 。 单 位 矢量 e 决定 了 直线 区 
的 方向 ,无 单位 ,而 n 决定 了 直线 的 位 置 ,n 的 模 表 示 直 线 到 原点 的 距离 。n RARE e 对 
原点 的 线 矩 (Moment of Line) ,可 以 这 样 理解 :在 直线 LL 上, 沿 直 线 工 方向 e 作用 一 个 单位 
力 ,n 是 其 单位 力 绕 着 原点 的 力矩 。 本 书 仅 使 用 规范 的 Pliicker 坐标 ,因此 Pliicker 坐标 用 
[e n] 表示 ,并 且 直 线 工 的 规范 Plücker 坐标 有 以 下 约束 条 件 :e *: e21,n* e-0, 





O 
图 5.2 过 两 点 的 直线 志 


5.2 刚体 的 一 般 运 动 


下 面 分 析 为 什么 刚体 在 空间 的 瞬时 运动 可 以 等 效 为 绕 沿 同一 轴线 的 螺旋 运动 。 如 图 
5. 3 所 示 ,4,P 为 同一 刚体 上 的 两 个 点 ,点 4 为 刚体 运动 的 参考 点 (或 描述 点 ) ,点 P 为 刚体 
上 任意 一 点 ,两 点 在 参考 坐标 系 下 的 原始 位 置 失 量 分 别 为 a,p ,刚体 在 空间 做 一 般 运 动 后 ， 
A,P 点 运动 到 点 4',P' ,此 时 位 置 矢量 为 a',p' ,刚体 在 空间 的 旋转 运动 用 旋转 矩阵 R fix, 
可 以 表示 为 
p'=a'+R(p-a) 13.2) 
从 式 (5.3) 可 以 看 出 , 当 给 定 a,a',p 和 R 时 ,可 以 确定 矢量 p', 即 可 以 得 到 点 PP 在 空间 
的 位 置 。 令 d ,dj 分 别 表示 点 4,P 在 旋转 后 的 位 移 , 表 示 为 
d,-a'-a,d,-p'-p (5.4) 
从 式 (5.3) 和 式 (5.4) 可 以 获得 d;, 即 
d,-a'-p«R(p-a)- 
(a'-a) *«R(p-a)-(p-a)- 
d,-( R-I)(p-a) (3. 2] 
从 式 (5.5) 可 知 ,刚体 上 任意 点 书 的 位 移 , 可 以 由 参考 点 4 的 位 移 、 刚 体 旋转 运动 R 和 
初始 位 形 下 的 矢量 p-a 确定 。 如 果 位 移 d, 已 知 , 则 矢量 p' 容 易 确定 。 根 据 欧 拉 定 理 : 刚 体 
绕 定 点 的 任意 有 限 转 动 可 以 由 绕 过 该 点 某 个 轴 的 一 次 有 限 转 动 来 实现 ,假设 刚体 旋转 矩阵 
为 R, 由 刚体 绕 空 间 单位 方向 为 e 的 轴线 转动 9 角 来 实现 ,参考 图 2. 19 , 式 (5. 5) 两 边 乘 以 
e ,得 
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e.d,-e'd,«e (R-I)(p-a) (5.6) 
因为 Re=e, 有 e =e R, 则 式 (5.6) 为 
ed,=e'd,=d, (5.7) 


式 (5.7) 说 明 , 刚 体 上 所 有 点 的 位 移 沱 着 轴线 e 有 相同 大 小 的 投影 长 度 。 





图 5.3 刚体 的 一 般 运 动 
【定理 $.1】 (Mozzi-Chasles) 给 定 一 个 刚体 的 一 般 运 动 ,存在 点 的 集合 构成 的 轴线 L, 
在 LL 上 的 点 具有 相同 的 位 移 最 小 值 。 而 且 ,轴线 工 和 最 小 位 移 方 回 与 刚体 旋转 轴线 方向 平 
行 。 
证 明 任意 点 P 的 位 移 d, ,可 以 分 解 成 两 个 垂直 的 分 量 ,参考 图 5.4, 即 平行 于 旋转 轴 
线 e 的 dy 和 垂直 于 e 的 分 量 d, , 则 


d =d; +d, (5.8) 
式 中 ,参考 附录 A 附 式 (16) 和 (17 ) ,dy =ee d, - dye,d , - (7-ee )dp。 显 然 
Id,l^ 21d l^ «Id, =d tld, V. (5.9) 


在 式 (5.9) 中 ,如 果 刚 体 在 运动 时 其 上 任意 点 P 的 1ds1 模 数 最 小 ,那么 只 能 令 1d ,| 为 
零 , 则 d, 平行 于 e, 此 时 对 于 瞬时 运动 , 扩 P 必然 位 于 轴线 e 上 , 仅 沿 e 做 平移 运动 ,位 移 矢 


量 为 
(5. 10) 





图 5.4 刚体 一 般 运 动 的 分 解 


上 面 推导 证 明了 Mozzi-Chasles 定理 第 一 部 分 , 即 存在 点 的 集合 构成 的 轴线 也 ,在 二 上 的 
点 具有 相同 的 位 移 最 小 值 。 第 二 部 分 轴线 工 平 行 旋 转轴 也 容易 证 明 ,如 果 刚 体 上 存在 一 个 
点 已 ” ,刚体 瞬时 运动 时 PI 点 位 移 具 有 最 小 值 , 则 垂直 于 e 的 位 移 矢 量 必 须 为 零 , 参 考 式 
(5.5) ,得 
d; -(I-ee')d,. - (I-ee' )d+(T-ee ) (R-I) (p' -a)-0 65.11) 
参考 式 (5.6) ,并 展开 进一步 推导 为 
(I—ee' )d, «( R-I) (p* -a)=0 (5. 12) 
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如 果 此 时 定义 一 条 轴线 工 通过 点 PI 且 平 行 于 e, 轴 线 上 所 有 点 的 位 置 天 量 可 以 定义 为 
p ”+Ae ,满足 式 (5. 12) ,可 知 所 有 最 小 位 移 的 点 位 于 此 轴线 上 。 

Mozzi-Chasles 定理 表明 ,刚体 的 一 般 空间 位 移 为 沿 空间 某 条 轴线 平移 和 绕 此 条 轴线 旗 
转 的 合成 ,如 此 平移 和 旋转 合成 称 为 螺旋 位 移 。 进 一 步 可 以 表述 为 :刚体 从 一 个 给 定 的 位 姿 
运动 到 任意 的 位 姿 ,其 运动 过 程 类 似 于 具有 轴线 L 和 螺 距 的 螺旋 运动 ,轴线 工 称 为 螺旋 
轴 , 瞬 时 do 为 运动 不 变量 ,因此 对 于 刚体 的 运动 ,可 以 定义 出 螺旋 轴 工 和 螺 距 ,其 螺 距 为 





式 中 ,h 的 单位 为 mrad, 
5.3 刚体 的 一 般 瞬 时 运动 
令 ay 和 po 表示 点 4,P 在 初始 参考 位 形 Cy 时 的 位 置 矢量 ,a(1) ,P(b) 为 时 间 为 纪 刚 体 


上 相同 点 在 位 形 C 的 位 置 矢 量 , 如 果 刚 体 开始 做 最 一 般 的 运动 ,其 所 有 点 都 是 动 点 ,在 初始 
点 4 处 建立 参考 坐标 系 ,如 图 5.5 所 示 , 则 P 点 的 位 置 撩 量 写 为 





图 5.5 刚体 一 般 瞬 时 运动 


p(t)-a(t) *R(t)(p,-a,) (5.13) 
式 中 ,R(1) 为 时 间 为 1 时 刚体 相对 于 初始 位 形 的 旋转 和 矩阵。 注意 , 式 (5. 13) 需要 初始 位 形 
C, 时 点 A,P 的 位 置 拓 量 , 点 PP 的 速度 通过 式 (5. 13) 两 边 对 时 间 微 分 表示 为 


p(t)=a(t) *R(1) (po-ao) (5. 14) 
进而 
y,-v,-0(p-a) (5. 15) 
写成 角速度 矢量 的 形式 为 
Vp-V,-tox(p-a) (5. 16) 
相似 于 定理 5. 1,5X (5. 16) 两 边 点 乘 w, 得 
w *Y»-o*v,-lv;llewl (Os 17) 
刚体 上 所 有 点 线 速度 在 角速度 方向 上 的 投影 相等 。 类 似 Mozzi - Chasles 定理 ,可 以 知 


道 : 当 刚 体 做 一 般 运 动 时 ,瞬时 存在 一 条 直线 二 ,位 于 此 直线 上 的 所 有 刚体 上 的 点 具有 相同 
的 速度 wy , 且 具 有 最 小 值 , 方 回 与 角速度 方 回 平行 ,此 直线 称 为 刚体 运动 的 瞬时 螺旋 轴 (I- 
stant Screw Axis ,ISA) 。 从 以 上 分 析 可 知 ,刚体 的 瞬时 运动 等 同 于 统 沿 ISA 的 螺旋 运动 , 随 着 
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刚体 在 空间 的 运动 ,螺旋 轴 也 要 变化 ,刚体 的 运动 称 为 瞬时 螺旋 运动 ( Instantaneous Screw 
Motion) ,因为 vy 平行 于 w, 故 可 以 写成 
y,-heo (5. 18) 
螺旋 轴 世 通过 Plücker 坐标 唯一 确定 ,定义 为 





式 中 ,e 为 直线 二 的 方 回 ;为 绕 原 点 的 线 矩 BI 


e-- 9 n-rxe (5. 19) 
r2 


A,r 为 螺旋 轴 上 任意 点 的 位 置 撩 量 。e 的 定义 是 唯一 的 ,但 在 刚体 瞬时 纯 平移 时 ,例如 ， 
瞬时 w=0,e 为 单位 天 量 且 平行 于 平移 运动 的 方向 。 在 几何 上 ,由 滑动 矢量 e 和 一 个 标量 配 
成 的 一 对 , 称 为 螺旋 (Screw) ,标量 h 称 为 螺 距 (Pitch of the Serew) 。 螺 旋 是 基本 的 几何 本 
质 , 而 运动 螺旋 是 刚体 运动 物理 的 阐述 。 瞬 时 螺旋 的 螺 距 h 可 以 定义 为 


所 或 者 1= ou 


ol € 
因此 ,刚体 的 瞬时 运动 可 通过 直线 Y^ h 和 转动 幅度 (Amplitude ) 1wo1 定 义 。 故 刚体 
的 瞬时 运动 可 由 六 个 独立 参数 确定 , 即 轴 线 工 的 四 个 Plicker 坐标 参数 一 个 螺 距 参数 和 一 
个 幅度 参数 。 


bp x e 








5.4 刚体 的 运动 螺旋 


在 这 里 为 了 和 其 他 著作 符号 保持 一 致 ,重新 定义 为 
$-6,9,-FXéÉ 
两 个 矢量 s 和 so 完全 定义 了 直线 荆 在 空间 的 方向 和 位 置 ,定义 一 个 包含 两 个 矢量 的 六 
维 数组 为 


s-|.| (5.20) 
称 为 线 矢 量 , 记 为 大 写 $ ,或 者 写成 对 偶 的 形式 为 
lien: d (5.21) 


AF ,s e s=1,s .so=0。 当 8 - $9250 时 ,是 比 线 和 失 量 更 为 一 般 的 情况 ,轴线 Plücker 坐标 参 
数 和 螺 距 参数 所 合成 的 六 维 数 组 通常 称 为 螺旋 , 记 为 $ , 即 


S e 
3E La s WS 


注意 , 式 (5.20) (5.22) PRE s,s 的 区 别 。 刚 体 的 角 速 率 乘 以 此 螺旋 得 到 刚体 的 瞬 
时 运动 矢量 , 称 为 运动 螺旋 (Twist) , 记 为 t。 通 常 一 个 Twist 可 以 认为 是 一 个 六 维和 矩 阵 , 完 整 
地 定义 了 刚体 的 速度 场 ,包含 刚体 上 任何 点 的 三 个 角速度 分 量 和 三 个 线 速度 分 量 。 如 图 5.6 
所 示 , 刚 体 瞬 时 螺旋 轴线 方向 为 s, 螺 距 为 h, 刚 体 上 一 个 点 0 的 指向 螺旋 轴线 的 矢量 为 x， 
瞬时 刚体 角 速 率 为 9, 则 此 时 点 0 的 速度 为 


(35.22) 
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o . HN 
eb HET =6| sons] QR 
式 中 ,09=1w|1,w=0s 为 刚体 角速度 ,其 方向 平行 于 s。 点 0 的 线 速度 为 
y -«x( —r) *he -0(rxs-hs) (5.24) 


9(rxs) 为 转动 引起 的 线 速度 ,垂直 s, 记 为 v, ,0hs 是 平 动 的 速度 , 记 为 wy ,其 点 0 的 线 
速度 是 螺旋 运动 合成 的 速度 。 





v=O(r Xs) 


图 5.6 运动 螺旋 示意 图 


参考 式 (5. 23) 可 知 ,已 知 刚体 上 某 点 的 速度 为 [Lz v], WR wz0, 即 s=w/lw| #0, 
则 在 以 此 点 为 原点 所 建立 的 坐标 系 下 ,刚体 瞬时 螺旋 运动 的 轴线 位 置 及 螺 距 为 
* XV | ul 
le^" lwl’ 
在 螺旋 运动 中 ,一 个 特殊 的 例子 是 螺 距 无穷 大 ,对 于 此 种 情况 ,本 书 采 用 不 太 严 格 的 
极限 的 方式 证 明 。 令 h— oo , 则 








(5.29) 


=( lim h) W (5. 26) 


h— œ 


im 


S s/h 
lim | | xi a| 
h>œ [rxs+hs] ^e (i(rxs)/h-s 
将 无 穷 螺 距 的 螺旋 定义 成 (0;s) 的 形式 , 称 为 偶 量 。 其 几何 意义 是 :在 法 向 量 为 8 的 平 
面 内 ,无 穷 远 处 直线 的 Plücker 坐标 。 此 时 ,对 于 任何 选择 的 原点 ,无 穷 远 处 的 一 个 无 穷 小 的 
和 天 量 ,对 原点 的 线 矩 缘 为 8, 与 原点 无 关 , 说 明 (0;s) 为 自由 矢量 。 


5.5 机 构 中 连 杆 基本 的 瞬时 运动 





5.5.1. 连 杆 的 瞬时 转动 


石 刚体 与 基 座 以 尺 关 节 相 连 ,瞬时 绕 着 关节 轴线 s 轴 转 动 ,如 图 5.7 所 示 , 转 动 的 角 速 
率 为 9, 在 刚体 有 上 一 点 0, 从 0 点 指向 旋转 轴线 的 矢量 为 r, 在 参考 坐标 系 |1} 下 ,刚体 的 0 
点 速度 表示 为 

w= ĝs 
Load (5.27) 

在 O 点 建立 一 个 坐标 系 ,其 方向 与 参考 坐标 系 11} 一 致 ,在 此 坐标 系 下 ,刚体 回转 轴 的 

RREN 
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rxs 


s-| «| (5.28) 
此 坐标 系 原 点 0 的 速度 为 | 
2: s» 


式 中 ,w 表示 刚体 转动 时 的 角速度 ;v 表示 刚体 0 点 处 的 线 速度 。 





图 5.7 刚体 的 瞬时 转动 


5.5.2. 连 杆 的 瞬时 移动 


香 刚 体 与 基 座 以 关节 尸 相 连 , 瞬 时 沿 着 关节 轴线 以 方向 8 移动 ,如 图 5. 8 所 示 , 其 速度 
的 矢量 为 ds, H d 是 移动 速率 。 对 于 移动 的 刚体 ,其 上 所 有 点 具有 相同 的 移动 速度 ,说 明 
RE s 的 平移 并 不 改变 刚体 的 运动 状态 。 刚 体 的 移动 速度 可 以 看 成 是 由 一 个 瞬时 转动 产生 
的 ,此 转动 轴线 位 于 单位 法 向 量 为 s 的 平面 内 , 距 s 无 穷 远 ,在 平面 内 ,位 于 无 穷 远 处 的 直线 
Plücker 坐标 为 (0;s) ,因此 刚体 瞬时 平移 速度 等 效 为 绕 此 无 穷 远 处 轴线 的 无 穷 小 的 转动 形 
成 的 速度 ,刚体 的 速度 可 以 表示 为 


(5. 30) 





图 5.8 刚体 的 瞬时 移动 


5.5.3 连 什 的 瞬时 螺旋 运动 


如 图 5. 9 所 示 ,空间 RP 机 构 , 在 末端 坐标 原点 0; 处 建立 一 个 与 基 坐 标 系 方向 一 致 的 
坐标 系 Fas ,在 此 坐标 系 下 , 杆 1 绕 关节 轴线 转动 ,瞬时 转动 为 9.[s， rixsi] , 杆 2 相对 杆 1 
做 平移 运动 ,瞬时 移动 为 d, [0 s,]" , 杆 2 的 绝对 瞬时 运动 是 两 个 运动 的 合成 , 肥 加 为 


ol em 
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去 (3. 31) 进一步 与 为 
e ô, 
PELIS n 
AF, $, $, 组 成 一 个 螺旋 系 , 即 
[ $, $] (5, 32) 
依据 式 (5. 25) , FF 2 瞬时 螺旋 运动 的 螺 距 为 
Dj d, 
ra y “S$, 二 一 


0, 0, 
式 中 ,a 为 两 轴线 的 扭 角 。 此 轴线 瞬时 在 坐标 系 Fs 下 的 位 置 矢量 为 


COS Q2 (3.33) 


d 
r=? Lsix(r xs) ex (s, xs,) (5.34) 
leo] 0, 


在 此 瞬时 , 杆 2 同时 具有 转动 运动 和 平移 运动 两 种 运动 特性 ,从 式 (5. 31) 可 知 , 杆 2 转 
动 角 速度 为 9s ,其 转动 轴线 的 方向 为 5, 杆 2 瞬时 沿 s, 轴 平 移 的 速度 为 vy = 0, hs, = 
d.,cos Q5 "Ti 0, Ab ZEE Jy I] 2g S] ,位 置 为 r 的 轴线 瞬时 转动 形成 的 线 速 度 为 | =, (rss, ) o 
当 s Ls: 时 ,两 轴线 的 扭 角 为 wz 290?,s, - 5,20, D (5. 33) rH, A 20, IEEE ET 2 瞬时 可 以 
等 效 为 纯 转 动 。 





图 5.9 刚体 瞬时 螺旋 运动 


5.5.4 机 械 臂 的 运动 螺旋 

通过 以 上 讨论 可 知 : 

(1) 当 有 =0 时 ,多 = Im ,刚体 可 等 效 为 纯 转 动 运动 , $ 为 运动 所 绕 直 线 的 规范 
Plücker 坐标 。 


0 
(2) 当 h=w 时 ,$= |] ,刚体 可 等 效 为 做 纯 移 动 ， 


GEIEN AF PS ,刚体 可 等 效 为 做 螺旋 运动 。 


(4) 34 s=0,rxs+hs=0 时 ,h 不 确定 。 
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当 所 有 的 关节 都 表示 成 运动 螺旋 时 ,串联 开 环 机 械 臂 未 端的 运动 就 是 所 有 运动 螺旋 的 
线性 组 合 。 与 第 4 章 的 雅 可 比 矩 阵 推导 过 程 比较 ,其 不 同 之 处 在 于 :在 未 端 运动 描述 点 建 
一 个 坐标 系 | .| ,方向 与 基 坐 标 系 一 致 ;@yr, 的 指向 变 为 从 所 建 坐标 系 原点 指向 坐标 屎 的 
原点 0;。n 个 关节 的 轴线 的 螺旋 在 坐标 系 | F.| 下 为 8 , $3,,… ,$3 ,, 第 i 关节 转动 的 速率 6 
映射 在 未 端的 速度 为 &. 8; ,这样 未 端 杆 的 运动 为 各 运动 螺旋 的 线性 组 合 ,机 器 人 未 端 瞬时 
运动 可 以 得 到 

Å, 
N =Å, $,+6, $240, $,2[ $B $, o $,] ] (5.35) 
ó, 
这 里 ,n 个 螺旋 ,[ $, S … $3$,] 构 成 了 一 个 螺旋 系 , 当 n<6 且 它 们 线性 无 关 时 ,此 n 
个 螺旋 就 是 一 个 n 系 螺旋 。 





图 5.10 到 2 关节 串联 机 械 臂 


5.6 螺旋 代数 运算 


1. 两 螺旋 的 代数 和 
两 螺旋 $, 和 8 ,其 代数 和 仍 为 螺旋 , 即 


S, +S, 
| (5. 36) 


$5 ^ 
$, +S, 
X] T PAAT- RERO ERA FR FELIS] 8E 00 — At 02 23 AER AE 2E ER ELE] JE , [EL t. n] E HH 
现 螺 距 为 零 的 螺旋 。 
2. 两 螺旋 的 互 易 积 (Reciprocal Product) 
两 轴线 在 空间 位 置 的 方向 关系 如 图 5. 11 所 示 ,两 个 螺旋 的 互 易 积 记 为 
$,»$,-s, 8S2+8， * s (5. 37) 
空间 两 个 螺旋 互 易 积 的 几何 意义 如 下 : 
(1) 两 线 矢量 互 易 积 为 
S] S2 
n PE Fa Mod 
r, XS, PXS 


S oS, =s * (r;X$,) ts, (rixs) 
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(r;-ri) * (s5xs,)- 
—a,58in a, (5. 38) 
由 以 上 分 析 可 知 ,如 果 两 条 直线 平行 , 则 on; =0, 互 易 积 为 零 ; 如 果 两 条 直线 相交 , GR 
离 为 零 , 互 易 积 也 为 零 。 总 之 ,两 条 直线 共 面 ,其 互 易 积 为 零 。 
(2) 两 个 螺旋 互 昂 积 为 
$° $, =S; © (r,xs,*h,s,) ts, * (r,xs,*h,s,)- 
(r,-r,) * (sxs,) *(h,*h,)s, * $= 
-a,58in a5 * (h, +h, ) cos ai; (5. 39) 
当 两 个 螺旋 互 易 积 为 零 时 , 则 它们 相 逆 , 称 8, 80$, t SCREEN GERE. RUNE 9 HJ 
反 螺 旋 记 为 多 。 从 式 (5.39) 可 以 看 出 ,两 个 螺旋 是 否 相 逆 ,与 瞬时 两 螺旋 螺 距 、. 连 杆 的 长 度 
及 扭 角 有 关 , 而 不 依赖 于 多 和 8& 所 在 的 坐标 系 , 即 与 坐标 系 无 关 , 这 简化 了 螺旋 应 用 于 自 
由 度 分 析 、 机 构 综 合 等 领域 坐标 变换 问题 。 





图 5.11 两 直线 互 易 积 


5.7 刚体 上 作用 的 力 螺旋 


5.7.1. 空间 任意 力 系 的 平衡 方程 


一 个 刚体 在 平衡 状态 (加 速度 为 零 ) , 当 且 仅 当 ,作用 在 物体 上 的 力 系 的 合成 为 零 , 对 于 
任意 一 点 力矩 合成 为 零 。 


(1) MRA ff, 是 作用 于 刚体 的 力 系 ,其 合成 力 为 = Y f. 
(2) MRS EARTEN s HEEREN Tr, = 0P,,f; 相 对 于 点 0 的 力矩 为 co =r; X fi, 
如 图 5. 12 所 示 。 相 对 于 点 0 的 合成 力矩 为 co = cu。 假定 作用 于 刚体 仅 为 力 ,没有 力 偶 ， 


但 也 没有 失掉 一 般 性 ,因为 力 偶 是 两 个 大 小 相等 .方向 相反 力 组 成 的 系统 。 空 间 任意 力 系 可 
以 问 任 意 一 点 的 简化 ,作用 于 刚体 上 的 空间 任意 力 系 万 万 ,应 用 力 平衡 定理 ,依次 将 
各 力 问 简化 中 心 0 点 平移 ,同时 附加 一 个 相应 的 力 偶 。 这 样 ,空间 任意 力 系 被 空间 汇 交 力 系 
和 空间 力 侦 系 co 两 个 简单 力 系 等 效 蔡 换 ,如 图 5. 13 所 示 。 一 旦 系统 简化 到 原点 ,不 失 一 般 
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性 ,就 可 以 定义 一 个 合理 的 参考 坐标 系 ,假设 f 的 方向 定义 为 z 轴 ,co 是 位 于 yz 平面 的 矢量 ， 
令 m, Fl c 表示 co 在 y 轴 和 :=z 轴 的 投影 ,如 图 5. 14 所 示 。 





图 5.12 刚体 上 力 的 表示 
进一步 思考 ,是否 能 发 现 一 点 4, 可 以 简化 力 系 |f,co| 到 点 4, 而 后 合成 的 力 偶 平行 于 矢 


Bf. WA 5. 14 所 示 , 显 然 存在 一 个 点 4, 当 力 f 平 移 到 点 4 时 ,其 伴随 力 偶 为 c=4A0xf= 
-mo ,因此 在 0 点 力 系 |f,co| 可 以 在 4 点 简化 为 等 效 的 力 系 |f,c} ,f,c 方向 平行 ,其 方向 用 
单位 矢量 s, 表示 , 沿 着 此 方向 的 直线 称 为 中 心 轴 , 推 导出 的 结果 称 为 潘 索 中 心 轴 原 理 
( Poinsot’ s Central Axis Theorem) 。 在 一 个 刚体 上 ,中 心 轴 是 唯一 的 ,不 能 在 刚体 上 发 现 另外 
一 个 点 其 合成 的 力 和 力 偶 方向 平行 。 





图 5.13 刚体 力 系 等 效 
六 维 矢量 wo。=[f co] 定义 为 原始 系统 力 fi ,f;,,…,f, 和 力 偶 ci,c,,…,c 在 0 点 合 
成 力 螺旋 (Wrench) ,可 以 在 任意 参考 坐标 系 下 合成 。 根 据 图 5.14 可知,f,c 方向 平行 ,可 以 
写 为 


c-hf 
式 中 ,h 为 囊 符 号 的 标量 。 与 运动 螺旋 类 似 ,wo 的 组 成 也 可 以 这 样 描述 ,包含 一 个 沿 $4 方向 
滑动 的 力 天 量 伴随 一 个 标量 h。 






N 
N 
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M 


c47A0: X f—-mo 


图 5.14 潘 索 中 心 轴 原 理 示 意图 
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如 果 给 定 O 点 处 的 力 螺旋 w。=[f co] ,可 以 求 得 螺 距 h 和 作用 线 s, 与 0 点 的 相对 
位 置 。 令 c=hlfl,c 是 co 在 f 所 定义 的 单位 方向 的 投影 , 写 为 














c=co Tf 
进一步 得 
aii E 
hifi =g fi 
F i Co 
h= (5. 40) 
因此 ,已 知 h, 可 以 得 到 c= 有 好 和 mo=co-c,wo 可 以 分 解 为 
TT 0 
Wo- | | 一 2] +| | = Im 到 (5. 41) 
式 中 ,r=04。 由 mo=rxf 可知 ,点 0 到 4 的 距离 为 
m, | 
rA (5.42) 
r 的 单位 矢量 为 
p aee (5.43) 
| fxm | 
则 
Imo Cfxm, ) 
LOTH ME fxmol Minis: 
显然 ,lxmol= MI Imol ,JEH fxm, =fx(co-hf) ,因此 
_fxco 
t gr (5.45) 


34 f c,20,H. Ifl #0 时 ,作用 于 刚体 的 力 系 是 纯 力 , 因 为 此 时 h=0, 即 c=0, 进 一 步 简 
EHR IS co) 到 中 心 轴 , 仅 获 得 一 个 力 矢量 了 而 无 关联 的 力 偶 。 此 时 co 垂直 于 f, 有 mo = 
Coo 

X Ifl z0 时 , 力 螺 旋 的 形式 为 wo=[0 c5] ,是 一 个 纯 力 偶 ,从 图 5. 14 可 以 看 出 ,如 果 
II 一 0 而 保持 co 为 常量 , 则 点 4 将 沿 着 x 轴 移 动 到 无 穷 远 处 , 螺 距 h 趋向 无 穷 。 从 这 个 意 
义 来 讲 , 纯 力 偶 就 是 一 个 螺 距 无 穷 大 的 力 偶 。 当 1fl=0 时 ,计算 中 心 轴 位 置 及 方向 没有 意 
Xo 

5.7.2. 刚体 上 作用 力 的 螺旋 表示 


1. 刚体 上 的 力 

刚体 上 的 作用 力也 可 以 用 螺旋 来 表示 ,如 刚体 上 有 一 个 作用 力 f, 如 图 5. 12 所 示 , 其 大 
小 为 f, 方 辐 为 s, 则 f= 记 , 沿 着 此 方向 的 直线 的 射线 坐标 , 即 Plücker 坐标 可 表示 为 [s rxs] ,r 
表示 在 参考 坐标 系 下 直线 上 一 点 的 位 置 矢量 。 此 力 对 坐标 原点 的 力矩 为 co。=rxfs =f(rxs)， 
在 0 点 刚体 的 力 螺旋 可 以 表示 为 
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f 8 
-| 
2. 刚体 上 的 力 偶 
作用 在 刚体 上 两 个 大 小 相等 .方向 相反 的 力 ,两 个 力 构成 一 个 力 偶 ,如 图 5. 15 所 示 ,与 
坐标 系 原点 的 位 置 无 关 , 是 空间 自由 矢量 ,可 以 表示 为 


r-r 
e- (rr )xfs-fir -ra| —— xs) 
Ir,-r 


; | 





r,-Tr, 





Jt 7S BANI efr -rl ATIS s= xs ,其 在 空间 的 任意 平 动 不 会 引起 刚体 


状态 的 改变 , 则 力 偶 可 以 表示 为 


Ir-r, | 


KH 





图 5.15 刚体 上 力 偶 螺旋 表示 


5.7.3 反 蝶 诈 的 物理 意义 
一 个 刚体 被 一 个 转动 副 约束 ,转动 副 轴 线 的 线 矢 量 用 S, = (s. ,so ) 表示 ,刚体 的 运动 可 


以 表示 为 
w ; .[ Si 
| | -ss = | 
Vo $01 


式 中 ,6 是 转动 的 角 速 率 ;w 是 转动 的 角速度 ;mw 是 与 原点 重合 点 的 线 速度 。 设 刚体 上 还 作 
用 了 节 距 为 零 的 力 螺旋 ,表示 为 

Fa $5 

|. -fs, i 


S02 
此 力 螺 旋 对 刚体 所 做 的 瞬时 功率 为 
P= 丰 yo+Co * w 
根据 式 (5. 37) ,两 个 螺旋 的 互 易 积 可 以 表示 为 
JS 0S| =f - v 4c, * c 
从 上 式 可 以 看 出 ,f5, ,0S, 两 个 螺旋 的 互 易 积 表示 力 对 刚体 瞬时 所 做 的 功 ,如 果 此 互 易 
积 的 值 为 零 , 即 
fS, »88, -0 
表示 力 螺旋 对 做 螺旋 运动 的 刚体 瞬时 所 做 的 功率 为 零 。 此 时 ,无 论 f 多 大 ,都 不 影响 瞬时 刚 
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体 的 螺旋 运动 ,为 刚体 螺旋 运动 的 理想 力 约 束 。 

【 例 5.1】 图 5.16 所 示 平 面 并 联机 构 , 动 平台 通过 三 条 RPR 运动 链 并 行 连接 于 基 座 ， 
移动 关节 为 驱动 关节 , 动 平台 有 三 个 自由 度 。 移 动 关 节 通 过 转动 关节 连接 动 平台 与 定 平台 。 
假设 三 个 驱动 关节 作用 力 的 大 小 为 有 «fa 和 ,在 参考 坐标 系 下 Oxy 下 ,作用 线 的 线 矢量 为 
$,,S, fll S, , 则 在 O 的 力 螺旋 为 

Wo- H -fi S, «f, S, «f, S, 
进一步 写 为 
S, S, S, 
vof E Jl 
$0 $0? $93 

—^r f, 的 垂直 力作 用 于 点 4, 计 算出 平面 并 联机 构 静 平衡 时 的 驱动 力 fi,f, I f, GJ 

在 0 点 的 力 螺旋 为 








图 5.16 平面 三 自由 度 并 联机 构 
【 例 5.2】 空间 2R 机 械 臂 包含 两 个 转动 关节 ,关节 1 驱动 刚体 在 水 平面 转动 ,关节 2 
垂直 于 关节 1 ,两 杆 杆 长 分 别 为 ol ,a; ,关节 变量 为 0,,0, , 试 分 析 在 坐标 系 11} 下 ,机构 的 自 
由 度数 与 运动 方向 。 在 基 坐 标 系 |1| 原点 处 ,关节 1 和 关节 2 的 运动 螺旋 为 


$1 
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刚体 G Æ O, 点 的 运动 螺旋 上 为 


0 S] 05s, 
0 一 C1 -6,c, 
t-t, +t, =Ê, : +Å, : - : 
0 0 0 
0 -Q, -Dai 
螺 距 及 螺旋 轴线 的 位 置 为 


| 0 * V _ -a1010, 
ww Å +Å; 
OXV 1 hi : 2e á : 2. T 
e 。 o E 102 I 102 I 0 
在 笛 卡 儿 基 坐标 系 |1| 下 ,构造 一 个 螺旋 为 $ , 令 运动 螺旋 上 和 所 构造 的 螺旋 史 的 互 易 
积 为 零 , 即 多 -ti=0, 8 为 运动 螺旋 上 的 逆 螺 旋 , 则 在 瞬时 ,螺旋 多 为 


$1 





Fe 


0 
$- EM (5. 46) 
Ó,s, 
0 
形式 时 互 易 积 为 零 。 式 (5. 46) RH, TESEAR TR ZR [1]. F, i x Ay 轴 , 绕 x ly 轴 存 在 理想 
的 力 约 束 , 只 有 沿 z 轴 , 绕 z 轴 力 做 功 不 为 零 , 即 沿 、 绕 z 方 回 存 在 运动 目 由 度 , 因 此 对 于 图 
5. 17 所 示 的 开 链 空间 机 构 , 其 自由 度 为 2, 即 沿 z 轴 平 移 和 绕 z 轴 转 动 。 





图 5.17 空间 2R 机 械 臂 自由 度 分 析 


本 章 小 结 


在 多 关节 串 并 联机 构 中 ,螺旋 轴 的 几何 意义 明确 ,为 关节 轴线 的 规范 Plücker 坐标 ,包含 
了 此 关节 轴线 的 方向 及 位 置 ,螺旋 系 中 各 螺旋 相关 性 的 判断 ,是 螺旋 理论 在 机 构 研 究 应 用 中 
的 基本 问题 。 本 章 通过 探索 一 般 刚 体 瞬时 运动 螺旋 和 瞬时 力 螺旋 描述 的 来 龙 去 脉 ,明确 其 
中 蕴含 的 物理 意义 ,来 引导 读者 用 螺旋 理论 分 析 机 构 。 
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轨迹 规划 的 主要 目的 是 为 运动 控制 系统 产生 期 望 的 输入 , 以 确保 机 械 臂 按照 规划 的 轨 
迹 运动 。 机 械 臂 的 操纵 者 指定 一 定数 量 的 参数 来 描述 期 望 的 轨迹 ,并 通过 捅 补 的 方法 把 期 
望 的 轨迹 按照 时 间 序 列 离散 成 一 系列 的 值 。 本 章 首先 研究 点 到 点 的 运动 ,然后 研究 多 后 运 
动 ,机 械 臂 的 运动 涉及 两 个 空间 , 即 关节 空间 和 任务 空间 。 机 械 臂 末端 的 运动 是 关节 运动 的 
映射 ,在 关节 空间 的 轨迹 规划 将 降低 规划 的 难度 和 控制 的 难度 。 

对 机 械 辟 最低 的 要 求 是 能 从 初始 位 姿 移 动 到 给 定 的 终止 位 姿 , 其 中 间 过 程 要 求 执行 器 
输出 广义 力矩 ,但 不 违背 执行 侨 固 有 的 物理 参数 ,如 执行 货 输 出 力矩 速度 及 加 速度 等 ,同时 
要 防止 出 现 机 械 臂 结构 的 共振 现象 。 在 茶 些 机 械 臂 中 ,驱动 关节 加 速度 的 变化 ,会 引起 振动 
现象 ,经 常 发 生 在 局 动 与 结束 过 程 中 。 因 此 有 必要 设计 轨迹 规划 算法 以 产生 平滑 的 运动 轨 
迹 。 通 党 末端 任务 路 径 规 划 以 后 ,需要 在 任务 空间 和 关节 空间 进行 轨迹 规划 。 图 6. 1 所 示 
为 两 空间 轨迹 规划 逻辑 关系 图 。 


任务 轨迹 插 补 
生成 当前 速度 及 位 姿 


———— 











—— —— Ó— — M ÀÀ 


图 6.1 两 个 空间 轨迹 规划 逻辑 关系 图 
机 右 人 路 径 (Path) 规 划 和 轨迹 ( Trajectory ) 规 划 是 容易 引起 混 消 的 两 个 术语 ,一 般 对 工 
业 机 全 人 而 言 ,路 径 规 划 表 示 在 给 定 机 械 臂 末端 初始 位 姿 和 目标 位 姿 , 在 无 碰撞 、 结 构 无 奇 
异 等 约束 条 件 下 ,按照 某 一 性 能 指标 (如 距离 .时间 、 能 量 等 ) 搜索 一 条 最 优 或 者 次 优 的 路 
径 。 轨 迹 规 划 是 对 于 所 规划 的 路 径 上 各 点 的 位 姿 、 速 度 和 加 速度 给 出 一 组 显 式 约束 (如 连 
续 性 和 光滑 程度 等 ) ,从 一 类 包含 时 间 变 量 的 函数 中 (如 多 项 式 等 ) 选取 参数 化 轨迹 ,对 各 点 
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进行 插值 并 满足 约束 条 件 。 
6.1 三 次 多 项 式 规 划 


在 任务 空间 中 ,规划 机 器 人 末端 的 路 径 是 直观 并 且 易 于 理解 的 ,例如 ,面向 具体 的 任务 
规划 其 初始 位 姿 、 目 标 位 姿 及 路 径 上 一 系列 过 点 ,并 且 给 定 完 成 此 路 径 的 时 间 , 但 在 任务 空 
间 实 现 期 望 控制 轨迹 是 困难 的 ,通常 需要 在 关节 空间 控制 各 关节 的 运动 ,那么 相应 的 轨迹 规 
划 也 在 关节 空间 进行 ,任务 空间 所 规划 的 路 径 各 点 通过 逆 运 动 学 映射 到 关节 空间 ,由 于 任务 
空间 与 关节 空间 转化 关系 的 复杂 性 ,对 末端 路 径 上 各 点 有 条 件 要 求 , 而 对 各 点 之 间 路 径 无 要 
求 的 任务 ,才能 在 关节 空间 直接 规划 。 本 书 只 研究 在 关节 空间 中 的 轨迹 规划 ,首先 讨论 点 对 
点 的 运动 。 在 点 对 点 的 运动 中 ,机 械 臂 某 关 节 从 初始 关节 位 置 go 运动 到 结束 关节 位 置 qr, 
时 间 路 度 为 4。 可 以 想象 出 ,如 果 没 有 约束 条 件 和 性 能 指标 的 要 求 , 在 要求 时 间 t, 内 ,存在 
无 数 多 条 平滑 曲线 满足 要 求 。 如 果 把 平滑 曲线 以 函数 9( 妇 表示 ,一 般 而 言 ,对 于 开始 和 结 
束 位 置 ,需要 满足 四 个 约束 条 件 , 即 

q(to)= qo,q(to)=vo 

q(tt)= qr,q (tt)= v 
式 中 ,vw ,vi 分 别 为 开始 与 结束 点 的 速度 。 即 使 给 出 约束 条 件 ,符合 此 约束 条 件 的 平滑 曲线 
也 是 数量 众多 ,因此 在 这 里 提出 能 量 最 小 为 此 轨迹 的 性 能 指标 ,假定 此 单 关 节 的 转动 惯量 为 
I, TET 8858 8 7J4B r 的 作用 下 ,从 qo 运动 到 gi, 单 杆 的 动力 学 方程 为 


la-T 


(6.1) 


式 中 ,a=g(i) ,需要 满足 的 约束 条 件 为 
OLETE? 
能 量 最 小 性 能 指标 可 以 表示 为 
min [oa 
经 过 推导 , 当 需 要 的 能 量 最 小 时 ,g(t) 必须 满足 曲线 
q(t)=at +bt+c (6. 2) 
当 关 节 速 度 符 合式 (6.2) 时 ,驱动 关节 完成 此 段 轨 迹 所 需要 的 能 量 最 小 ,同时 从 式 
(6.1) 也 可 以 看 出 ,此 段 轨 迹 需 要 同时 满足 四 个 约束 条 件 , 如 果 采 用 多 项 式 为 轨迹 规划 的 函 


数 至 少 也 要 为 三 次 多 项 式 ,所 以 从 两 方面 来 看 ,三 次 多 项 式 规划 关节 的 轨迹 是 合理 的 选择 。 
为 了 规划 关节 的 轨迹 ,可 以 选择 的 三 次 多 项 式 为 


q(t)= ast *a,t +a,t+a, (6.3) 

沿 该 轨迹 的 关节 速度 、 加 速度 表示 为 
q(t) 2 3a4t^ 42a,t«a, (6.4) 
q(t)- 6a,t*2a, (6. 5) 


将 四 个 约束 条 件 代 入 式 (6.3)、(6.4) ,得 
qo — a4, tat +a tatao 


do = 3a.t; +2a,to +a, 
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q, —a4t; *a,t; *a, t.a, 
qe 23a,1t; -2a,t,*a, 
上 面 四 个 公式 写 为 矩阵 的 形式 为 


l| & € Gk do 
一 (6. 6) 


0 1 2n 3üj|le qr 
【 例 6.1】 单 杆 转 动 关 节 机 器 人 ,在 1s 内 由 起 始点 q(0)- 20° 开 始 运动 到 终点 q(1)= 
-10", 且 起 始 与 终止 速度 均 为 零 ,运用 三 次 多 项 式 规 划 该 机 顺 人 关节 运动 轨迹 ,假定 择 补 周 
期 为 2 ms, 画 出 关节 位 置 .速度 与 加 速度 的 变化 曲线 。 把 上 述 约 束 条 件 代 和 人 式 (6.6) ,得 出 
矩阵 形式 为 


1 0 0 olfa] fo 
0 1 0 Olla! | 0 
L Lob gdüllsl. 
U L 2 3JlaJ..L0 


解 得 
a, 7 q, ,a, 20,2; 23(q,-7q4,) ,as - -2(q,-49) 
所 得 到 的 三 次 多 项 式 的 图 数 为 
q(t)= go+3(g-qo)t -2(g1-qo)t ,te[0 1] 
依据 此 函数 ,以 时 间 间 陋 为 2 ms 规划 得 出 的 关节 人 位置、 速度 和 加 速度 随时 间 变 化 的 曲 
线 如 图 6. 2 所 示 。 


20 0 200 
-5 
" 150 
-10 100 
10 -总 Q0? 
Bri ji 70 0 
po^ m -25 m 
0 -30 -30 
ee -100 
= -40 —150 
* -4 k 
10 0.5 l ”0 05 0.5 l 
IM /s 时 间 /s 时 间 /s 


图 6.2 三 次 多 项 式 规划 的 位 置 .速度 和 加 速度 


6.2 直线 与 抛物 线 混 合 规划 


在 工业 机 右 人 实际 应 用 中 ,经常 采 纳 直 线 与 抛物 线 混合 的 轨迹 规划 方法 ,此 方法 的 优势 
在 于 ,可 以 直观 发 现 所 规划 轨迹 的 速度 和 加 速度 是 否 在 机 器 人 物理 参数 约束 条 件 范围 内 ,在 
这 种 方法 中 ,速度 曲线 为 梯形 曲线 ,在 关节 运动 过 程 中 ,开始 以 固定 值 加 速度 加 速 运 动 ,然后 
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以 匀速 运动 ,最 后 以 固定 值 加 速度 减速 运动 到 达 目 标点 。 其 位 置 曲线 包含 直线 段 ,两 端 通过 
抛物 线 连接 到 开始 点 和 结束 点 ,如 图 6. 3 所 示 。 





0 而 fh ft kr í 





图 6.3 直线 和 抛物 线 混 合 位 置 .速度 与 加 速度 特征 线 
本 书 所 给 的 轨迹 规划 起 点 与 终点 的 运动 ,给 定 延 续 时 间作 为 约束 条 件 ,而 在 工业 机 器 人 
实际 应 用 中 ,用户 往往 给 定 最 大 容许 速度 的 百分比 ,这 是 因为 如 果 给 定 延 续 时 间 过 短 ,规划 
的 速度 与 加 速度 过 大 ,可 能 超出 机 融 人 的 能 力 范 围 。 从 图 6. 3 中 可 以 看 出 ,此 种 轨迹 规划 方 
法 开始 与 结束 速度 ,加 速度 为 零 ,开始 与 结束 轨迹 段 中 加 减速 过 程 有 相同 的 时 间 延 续 , 即 加 
速 \ 减 速 过 程 的 加 速度 大 小 相等 。 匀 速 运 动 的 最 大 速度 给 定 为 v., 加 减速 大 小 给 定 为 ae。 
假定 开始 时 间 为 ,=0 ,结束 时 间 为 t+, 直线 与 抛物 线 开始 混合 的 时 间 为 i,。 从 图 6. 3 中 也 可 
以 看 出 ,其 位 置 轨迹 曲线 在 i,=i/2 处 ,轨迹 点 位 置 为 qu=(9f+qgo)jX2。 当 给 定 go,qr ;二 时 , 首 
先 研 究 轨迹 规划 所 给 定 的 加 速度 必须 满足 的 约束 条 件 ,以 确保 给 定 延 续 时 间 t, 下 ,恰好 从 

开始 点 qo 运动 到 qi, HE t, 时 刻 混 合 时 ,其 速度 必须 满足 的 条 件 为 





ges (6. 7) 
AF q, 表示 在 混合 时 间 t, 时 ,以 常量 加 速度 a. 加速 运动 到 的 位 置 点 ,可 以 表达 为 
gs 74, af (6.8) 
根据 式 (6.7) 和 式 (6.8) ,JF ELA ta =ti/2, 可 以 得 出 
a ti-a, tit,+qe—qo=0 (6.9) 
当 给 定 do ; Q1, tt, Oe 时 ， 并 且 要 符合 加 St; /2 条 件 Lt, 求解 得 
3 dy 在 4(qi-90) 
zie " x^ 4 m a. (6. 10) 


显然 ,从 式 (6. 10) 可 以 看 出 ,ae 的 正 负 号 sgn(a。) 要 与 sgn( qi-qo) 一 致 ,并 且 给 定 的 加 
速度 要 符合 以 下 约束 条 件 : 
t 4(gr-go) 
Q. 


4lqi-qo | 
4 2 


f 


>=0, E] læ. l| = (5. 11) 
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当 给 定 的 加 速度 的 大 小 满足 式 (6. 11) 的 等 式 时 ,在 此 轨迹 中 仅 有 加 速 、 减 速 阶段 ,无 勾 
速 阶段 。 直 线 和 抛物 线 混合 的 轨迹 线 可 以 表示 为 


qa. 0 xxt, 
1 
q(t)- q,*a,t (175 Q, AiE (6. 12) 
1 2 
J= e t-t), tet tSt; 


确定 轨迹 加 速度 约束 条 件 , 不 是 梯形 速度 曲线 轨迹 规划 的 唯一 方式 , 当 给 定 qos 41. t 
时 ,根据 式 (6.7) 可 知 v.=ais ,代入 式 (6.9) ,ts 表示 为 
"ue mo ue (6.13) 


V. 


必须 满足 的 约束 条 件 为 


Bp 
| qq | 


Qs Dis, 1:8 
t; tt 
因而 , 当 给 定 匀 速 运 动 速度 vv 时 ,常量 加 速度 为 
v. 
& = 
qo -4(*V.t, 


通过 式 (6. 14) 〈6. 15) 计算 得 到 与 ,ae, 而 后 可 以 通过 式 (6. 12 ) 随时 间 变 化 产生 一 系列 
的 插 补 上 后。 


(6. 14) 





(6. 15) 


6.3. & ARER 2 Iz x 


在 许多 应 用 场合 ,为 了 避 开 运动 过 程 中 的 障碍 物 或 者 使 实际 运动 轨迹 更 符合 期 望 轨迹 ， 
机 器 人 的 路 径 需 要 通过 许多 点 来 描述 ,如 图 6. 4 所 示 。 如 果 机 器 人 路 径 规 划 所 得 出 的 点 为 
n+l 个 ,并 且 规 划 好 过 每 个 点 的 时 间 ,如 果 所 有 的 点 有 m+1l 个 约束 条 件 , 则 至 少 需 要 n 次 多 
项 式 才 能 满足 n1 个 约束 条 件 ,但 是 会 产生 以 下 问题 :中 对 每 个 点 处 的 速度 分 配 困难 ;@) 多 
项 式 阶 次 越 高 ,曲线 特性 越 可 能 振荡 ;(3 多 项 式 系数 的 解 随 着 阶 次 提高 精度 变 差 , 甚 至 无 法 
求解 ;9 多 项 式 系 数 求 解 以 后 ,如 果 更 改 路 径 中 的 点 ,需要 重新 计算 。 

为 了 克服 上 述 缺 点 , 插 补 所 采用 的 多 项 式 为 低 阶 次 的 多 项 式 ,对 于 n+tl 个 点 ,关节 轨迹 
规划 的 多 项 式 函 数 g(t) ,可 用 nn 个 三 次 多 项 式 函数 (1) (其 中 k=0,1,2,…,n-1) 组 合 而 
成 ,函数 q(t) 在 [—t, 时 刻 到 达 各 个 点 d, ,并 且 满 足 终 止 条 件 t, =tr,g (tr)= gro 首先 研究 存 
在 速度 约束 的 插 补 多 项 式 , 此 种 方法 要 求 用 户 指 定 每 个 过 点 的 速度 ,与 两 个 点 的 三 次 多 项 式 
轨迹 规划 没有 区 别 , 所 规划 路 径 中 每 两 个 规划 点 的 三 次 多 项 式 插 补 函数 需要 满足 如 下 约束 
RIF: 
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N 


do 


ea a 


O0 h b ha Gh t 
图 6.4 Zu XB BST 
lt.) 7 q, 
lta) 7 414 
,8z0,1,2,5», 5-1 (6. 16) 
l,(t,)7 q, 
AUT ) = di 
存在 n+l 点 ,有 nn 组 如 式 (6. 16) 所 示 的 三 次 多 项 式 函 数 的 四 个 未 知 系数 需要 求解 ,一 
般 的 ,开始 与 终止 的 速度 设置 为 零 , 即 go=q,=0, 同 时 为 了 满足 速度 连续 的 约束 条 件 ,两 个 
相 邻 的 多 项 式 函 数 要 满足 以 下 约束 条 件 : 
AER )= kalba ) ,kz0,1,2,*--,n-2 
为 每 个 过 点 分 配 速度 不 是 一 件 容 易 的 事情 ,尤其 过 点 数量 较 多 时 ,一 种 方式 是 在 每 个 点 
处 为 速度 设 定 一 个 规则 ,可 以 设 定 为 


qi -0 
0, sgn( v, ) z Sgn (Vy ) 
aedi vea), sgn(v,)- sgn(v,,,) Popes een 
q,-0 
AP v= (09,7915 )A(ti-ti ) 为 两 点 线段 相对 于 时 间 的 斜率 ,其 物理 意义 为 [tj 间隔 内 


匀速 度 运动 。 
6.4 多 操 直 线 与 抛物 线 混 合 规划 


当 已 经 规划 好 关节 空间 内 n+1 个 径 点 go,q1,…,g,, 并 且 给 定 经 过 各 个 点 的 时 间 to, 
t, vnu, ,轨迹 规划 采用 直线 插 补 方式 ,为 了 避免 在 时 间 六 时, 即 速度 不 连续 问题 ,在 两 条 直 
线段 之 间 采 用 抛物 线 平滑 过 渡 的 方式 ,关节 轨迹 函数 qC) 在 时 间 t 附近 平滑 过 渡 , 所 规划 
的 轨迹 包含 了 直线 段 和 二 次 多 项 式 , 但 加 速度 不 一 定 连续 。 如 图 6. 5 所 示 , 令 An =tiw -ti， 
At, ,为 从 qu 到 qi 轨迹 插 补 中 直线 插 补 段 的 时 间 延 续 , 令 Graa DA qu 到 qi, 匀 速度, 并且 
d, 为 抛物 线 平滑 过 渡 时 间 At, 延 续 内 的 加 速度 。 与 单 段 路 径直 线 与 抛物 线 混合 规划 一 样 ， 
此 种 方法 中 值 qus Ac, At 需要 路 径 规划 时 给 定 ,各 中 间 点 的 速度 和 加 速度 为 


. di di 
dk-1,k At, 





(6.18) 
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和 (6. 19 ) 

对 开始 和 结束 线段 需要 特别 注意 ,考虑 起 始点 和 终止 点 的 抛物 线 过 渡 时 间 ,如 图 6.5 所 

示 , 应 该 全 部 计 和 人 轨迹 规划 总 的 延续 时 间 为 t,to+0. 5(to+is) ,速度 约束 为 9_1,0=9.nw1 =0。 
x Qi At, ,Ai 给 定时 ,qi jd; H1 3X (6. 18) .(6. 19) 计算 得 出 。 如 图 6. 5 所 示 ,g(t) 不 一 定 


通过 所 规划 的 路 径 点 ,而 是 在 其 附近 ,混合 时 的 加 速度 越 大 , 越 接 近 规划 的 路 径 点 。 





Q.5t0t0 t b t =f; QS I 


图 6.5 多 点 直线 与 抛物 线 混合 规划 
【 例 6.2】 单 杆 旋转 关节 机 器 人 ,由 初始 位 置 qo 70,3825 q, =30°,g,=20°, 到 达 终 点 
q3=5°, 时 间 (20,5, 22,0; 23,1, =4, 初 始 结束 速度 为 do -0,q, =0, 各 线段 混合 的 时 间 为 
0.2 s, 求 混合 时 的 加 速度 。 
总 延续 时 间 为 i,-to+0.5(to+ttm)=4.2, 初 始 ,中 间 各 线段 及 结束 速度 分 别 为 


: : d1 一 4 á d2 一 4 ó d3 一 4 5 
q.1070,49,7 r *=15 ( )/s ,91 > = E ---10( ) /s ,92 3 = E “=-5 ( )/s,q3.4=0 


混合 时 间 为 0. 2 s, 混 合 时 的 加 速度 为 
Pi  Qoi7d3ip o " -Qia-7doa o 
CANC HE, 2751 )/s,q1 y, -—-125 (?)/s 











do 
y a aeui m a Tur cu va 
d= 0.2 =25 ( WEGE 0.2 =25 1 )/s 
a e 
Ax NE 


本 章 主要 人 研究 了 关节 轨迹 规划 的 问题 ,涉及 三 次 多 项 式 规 划 直线 与 抛物 线 混合 轨迹 规 
划 ,以 及 多 点 的 三 次 多 项 式 和 直线 与 抛物 线 混 合 轨迹 规划 时 中 间 点 速度 与 加 速度 分 配 的 问 
题 ,所 人 研究 的 方法 在 实际 工程 中 有 较 强 的 应 用 性 ,同时 为 深入 人 研究 其 他 关 市 空间 内 的 轨迹 规 


划 方法 葛 定 了 良好 的 基础 。 
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机 器 人 动力 学 方程 或 运动 方程 (Equation of Motion) ,是 研究 当 作 用 于 关节 上 驱动 力 或 
力矩 时 ,机 器 人 的 运动 状态 。 所 推导 的 动力 学 方程 或 所 建立 的 动力 学 模型 描述 了 机 骨 人 动 
态 行为 ,在 以 下 几 个 方面 有 重要 的 作用 : 

(1) 动 力学 模型 用 于 开发 适合 的 控制 策略 ,一 些 控制 方案 直接 依赖 于 动力 学 模型 ,为 了 
实现 在 驱动 力 或 者 力矩 作用 下 期 望 的 运动 轨迹 。 

(2) 动 力学 模型 用 于 机 器 人 系统 计算 机 仿真 ,通过 模拟 在 不 同 条 件 下 的 模型 行为 特性 ， 
预测 机 器 人 系统 搭建 完成 后 的 行为 。 

(3) 给 定 机 龙 人 关 届 驱动 力 和 力矩 ,可 以 动态 分 析 机 胡 人 系统 最 优 的 杆 件 的 尺寸 形状 、 
轴承 承载 力 等 相关 参数 。 

机 磊 人 动力 学 分 为 逆 动 力学 和 正 动力 学 两 方面 。 逆 动力 学 是 分 析 机 器 人 沿 期 望 轨 迹 运 
动 所 需要 的 关节 驱动 力 或 力矩 ,其 方程 用 于 机 器 人 运动 控制 。 正 动力 学 是 当 作 用 于 关节 驱 
动力 或 者 力矩 给 定时 ,机 器 人 系统 的 啊 应 (一 般 包 括 位 置 、. 速 度 及 加 速度 ) ,主要 用 于 系统 的 
计算 机 仿真 。 机 器 人 系统 的 动力 学 建 模 有 众多 方法 ,包括 牛顿 - 欧 拉 法 、 拉 格 朗 日 法 、. 凯 恩 
方程 等 ,为 了 便于 理解 作用 于 机 械 臂 各 杆 件 的 作用 力 ,首先 从 研究 机 器 人 静 力 学 开始 。 


7.1 IF 


工业 机 器 人 在 狼 配 生产 线 应 用 时 ,其 末端 要 与 外 部 环境 接触 ,类 似 的 场合 还 包括 灵巧 手 
抓 取 以 及 行走 机 货 人 腿 都 要 和 周围 环境 接触 ,所 有 这 些 场合 都 需要 控制 机 器 人 未 端 和 环境 
之 间 的 接触 力 。 本 节 首 先 讨 论 机 器 人 在 静态 时 末端 作用 于 外 部 环境 的 力 和 力气 及 各 关节 驱 
动力 之 间 的 关系 。 


7.1.1 单个 杆 件 静 力学 


首先 考虑 在 开 链 工业 机 器 人 机 构 中 单个 杆 件 区 不 为 基 座 和 末端 杆 ) ,通过 关节 i 和 i+1 
连接 杆 i-1 和 杆 ils Ed 7. 1 所 示 为 作用 在 杆 件 i 上 的 力 和 力矩 ,0; 是 坐标 系 F 的 原点 ,此 
坐标 系 固 连 于 杆 i-1, 位 于 关节 i。 杆 i 前 后 两 端 受 到 相 邻 杆 通 过 关节 作用 的 力 和 力矩 。 
Sii EZERRE, RIT i-1 作用 于 杆 i 的 力 ,fi ,1 表示 杆 i 作用 于 杆 i+1 的 力 , 则 杆 i+1 反 
作用 于 杆 i 的 力 为 -f; ;it。ni 1 是 三 维 矢 量 , 表 示 杆 i-1 作用 于 杆 i 的 力矩 ,n, ;表示 杆 i JE 
用 于 杆 i+1 的 力矩 , 则 杆 i+1 反作用 于 杆 ; 的 力矩 为 -Pi; is o JJ fi1; 和 力矩 n, ; 称 为 杆 i=l 
和 杆 i 间 关 市 力 和 力矩 。 杆 i 的 重力 位 于 质心 表示 为 mg, m, 表示 杆 i 的 质量 ,g 是 三 维 矢 
量 , 表 示 重 力 加 速度 。 
在 惯性 坐标 系 下 , 杆 i 静 力 平衡 方程 为 
Jiii fii tmg=0,i=1,,n (7. 1) 
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图 7.1 单 杆 静态 时 受 力 示 意图 
相对 于 质心 点 Ci, 其 静 力 矩 平衡 方程 为 
Miia Th ua 一 人 大 isi trac) xf; Lit Fisic) x( f. )=0, iz1,:-:,n (7.2) 
AP, r i 是 位 置 和 拓 量 ,在 基 坐 标 系 下 从 点 0; TRIS) SR 0 ,Trinic, 表 示 从 点 0 指向 杆 守 的 质 
DA Ci。 关 市 力 和 力矩 为 有 和 no ,表示 基 座 , 即 杆 0 和 杆 1 之 间 的 关节 力 和 力矩 ,如 图 
7.2(a) 所 示 。 关 市 力 和 力矩 为 fn 和 ni ,表示 末端 杆 , 即 杆 n 和 外 部 环境 之 间 的 作用 力 
和 力矩 ,如 图 7.2(b) 所 示 。 当 机 器 人 末端 接触 环境 时 ,作用 于 末端 的 反作用 力 为 -f, ,和 


= n o 





(a) 
图 7.2 杆 1 和 杆 n 受 基 座 和 外 部 环境 的 力 
ADR DL A BUE BUR n+l 个 杆 件 (包括 基 座 ) ,可 以 列 出 2n 个 瞬时 矢量 方程 ,但 方程 中 
包括 2(n+1) 个 未 知 矢量 。 因 此 ,为 了 求解 方程 中 关节 力 和 力矩 ,必须 给 定 一 组 关节 力 和 力 
AB ,否则 方程 无 法 求解 。 力 记 和 力矩 n, ,是 在 机 器 人 任务 中 指定 作用 于 外 部 环境 的 力 
和 力矩 ,因此 给 定 此 对 力 和 力矩 后 ,可 以 求解 以 上 瞬时 方程 。 为 了 方便 起 见 , 把 力 和 力矩 定 
写成 力 螺旋 的 形式 为 
fau. 
vase | je n 
n; , , 


式 (7.1) 和 式 (7.2) 可 以 写 为 矩阵 的 形式 为 
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foi 
E 0 -E 0 "— 0 0 n, 1 
S(-(ro.,*r4,,)) E Slrie) -E = 0 0 || fiz 
: : : || ma |= 
0 0 0 0  … E 0 i 
0 0 0 E o Wiaan El fa. 
WF, oin 
-m,g 
0 
-M8 
0 05.3) 
fom ThE 


n, nd Tu ,Ci Xf ui 


式 中 ,EeR” 表示 单位 和 矩阵,S(v) 表 示 矢 量 v 构造 的 斜 对 称 和 矩阵 。 
7.1.2 递归 法 计算 关节 力 


为 了 方便 求解 关节 力 和 力矩 , 式 (7.1) 和 式 (7.2) 写 成 递归 的 形式 为 
fi1i=fiin —mig 
Miia ua TP Lua trac) Xf i i7 iac Xf ii sb 5, (7.4) 
矢量 方程 (7.4) 在 任何 参考 坐标 系 下 都 成 立 。 为 了 计算 方便 ,矢量 7 ,, ,ri,iic 经常 表 示 
在 坐标 系 |i+1| 下 ,此 坐标 系 与 杆 i 后 问 固 连 ,此 时 [ rr; iga Lid» [有 二 六 党 是。 fExX B [rr], 
书写 方式 表示 7 矢量 以 坐标 系 F, 为 参考 。 第 i 杆 的 静 力 平衡 方程 (7. 4) 在 坐标 系 |i+1| 下 
写 为 
LE ds ] sp LE. sa -mg | i+l 
[ni ji = [Wi st (Fa Hsc, ) Xf; A iT iac Xia lii i2n,951 (7.3) 
在 坐标 系 1i+1} 下 求 得 [fi1; ji o liile ,转化 到 坐标 系 | 计 下 的 方程 为 
Lf .::J; 7g, Lf uda 
[5,51]; 20, E EFIT (7.6) 
如 有 果 重 力 加 速 天 量 8 以 及 末端 作用 于 外 部 环境 的 力 和 力矩 . 几 I mw 在 参考 坐标 系 
i1} 下 指定 , 则 通过 弟 推 的 方式 得 到 在 坐标 系 |i+1| 为 
[21.4 7Q; [Lg]i,i=1,.…,n 
LE od lad ap pw la FIL n, n lai = cw li (I.T) 
XB JJ fi MIHE n; 1; 是 作用 于 关节 的 合成 力 和 力矩 ,包含 了 理想 约束 力 和 力矩 及 电 
机 等 执行 器 作 用 于 关节 的 主动 力 或 力矩 。 令 单位 矢量 e; 轴线 表示 关节 i 轴线 的 方向 ,如 果 
第 i 个 关节 为 转动 关节 ,执行 器 绕 关节 轴 产 生 的 主动 力矩 为 7; 是 关节 力矩 ,n; 1 ; 沿 着 轴线 方 
问 e; 的 一 个 分 量 ,表达 式 为 
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T;-c€; "Mi, (7. 8) 
如 果 第 i AUS LUI DUET SRI E — 1 2] 18 72365882373 [807 7;, 是 力 fT EI 
的 方向 的 一 个 分 量 ,表达 式 为 
T;26; * fi, (7.9) 
注意 ,为 了 统一 ,在 转动 关节 和 移动 关节 中 使 用 了 相同 的 符号 表示 ,但 关节 类 型 不 同 , 标 
量 r; 的 单位 不 同 。 
【 例 7.1】 平面 2R 串联 机 械 臂 ,如 图 7. 3 所 示 , 各 杆 的 长 为 ol ,az ,各 杆 质心 点 Ci, C 
位 于 各 杆 的 中 心 。 各 坐标 系 的 建立 参考 第 3 章 ,在 坐标 系 13} 下 ,已 知 机 械 臂 末端 作用 于 环 
境 的 力 为 了 ,在 静态 时 , 求 所 输入 的 主动 力矩 7, 和 7,。 





图 7.3 2R 机 械 臂 静 力 分 析 
假设 在 坐标 系 131 下 ,末端 对 外 部 环境 施加 的 力 及 重力 g 在 参考 坐标 系 下 的 矢量 为 



































Ís 0 0 
[f2]; = b , [7055]. 0 es (7. 10a) 
0 0 0 
坐标 系 之 间 的 旋转 矩阵 Q, Q, 为 
c, -s 0 c, =% 0 
Q-|s co 0|,0,=|s, o. 1 
0 O0 31 0 0 1 
在 坐标 系 12} ,13| 下 ,各 几何 参数 和 重力 的 矢量 为 
d, A —a,/2 —a5/2 
nea E025 |3= 0 Lra 127 0 [s 0 | 
0 0 0 0 
—831 -8512 
[2],-Q; nem ,[g]5-Q; eom (7. 10b) 
0 0 


依据 式 (7.5) 可 知 
[fia 159 Uf 7 m4g 13, [ma ]5 9 L0 49 (r2 trs 5) Xf 2773, 5,251, 522 (7. 10c) 
将 式 (7. 10a) (7. 10b) 代 和 人 式 (7. 10c) 得 





f, mags x 
[fi2]1s7 f,*m;ge ,[ a] = 
0 af, Ta a. 
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[fial , [15]; 变换 到 坐标 系 12 上 下 为 
fc2 f, 5, * m,gs; 
Lf slo -Q, [£12]; = f 53 Xf, c; +m go 
0 
0 


[m 5;];2Q,[ mi], ; 


af, ta 380261 


主动 力 T2 为 
7;2|e€;]; $ [n;5],;-[0 0 1] [m]; =f, maga, c 


同 理 
ET h= [fi2-m8]: , [i h= [Mia tira ta ) Xfo -7r2 u xf, 2]; „i=l 
则 
f. f 55 * (m, *m, ) gs, 
[53 jz= fs2+f,c2+( ms *m, ) gc: 
0 
0 


0 
[mo |, = 


1 1 1 
af, V9 388261 *tajf,s; *a,f,c; t3 Mngt C Ta 80 C 


frc12 -fys12 
[foi ] ı =Q, Efe Ja - fS Xf ei * (m, m, )g 
0 


静 力 学 内 向 递归 计算 流程 见 表 7. 1 
表 7.1 静 力 学 内 向 递归 计算 


给 定 f, stus 
9o 建立 D-H 坐标 系 ,在 所 建立 的 局 部 坐标 系 下 给 出 [ri [Fi cia ];,i22, "ntlin 为 杆 件数 ) ,计算 出 
Q, ~ 0, ,给 定 [g] iT Lal; - Lal.a 
for izn to 1 step -1 do 
Ufa lii lf; is -mg | isl 
[nili I niat rutru) Siri Pira Sii la 
% 在 计 1 坐标 系 下 求 得 [fi1;]i o liii li ,转化 到 i 坐标 系 下 的 方程 为 
Lfaulie9; lfi- lin 
[miii liQ; [Ln] 
if R 关节 then 
vieil; * [na]; 
else 


T;«—| e; ]; * [fail 
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end do 


0 


[2551], 2Q, [75], d 


1 l 
af, +a,f,S2+0,f c; m Ba *tm»;ga,c, Ta leg. 
主动 力 Ti 为 
1 1 
mi=le d” [ 1o. Is -ajf,*af,s; ta,f,c; Ta hn *m»;ga,c, Th Thal8s€1 


写成 矩阵 的 形式 为 


l 
mga, c, +M, gac pra m» £C, 


T| G,$, G,€C,t*0G, 
cH mm cd 
T2 


I 


7.1.3 能 量 方法 


在 7.1.2 节 中 ,基于 静 平 衡 力 所 列 的 瞬时 方程 求解 困难 ,在 复杂 的 机 器 人 系统 中 能 量 方 
法 建立 关节 力 和 末端 力 之 间 关 系 是 理想 的 选择 。 在 能 量 方法 中 ,系统 的 描述 基于 能 量 和 功 ， 
因此 在 关节 力 和 力矩 中 不 产生 储存 和 耗 散 能 量 的 项 在 方程 中 排除 ,这 些 被 排除 的 项 称 为 理 
想 的 约束 力 和 力矩 。 为 了 应 用 能 量 方法 ,首先 需要 确定 主动 力 和 力矩 ,其 次 发 现 系统 的 广义 
坐标 。 在 关节 空间 中 ,关节 主动 力矩 定义 为 r=[7，7， … n] ,此 矢量 表示 执行 器 作用 
于 关节 上 的 力 或 力矩 ,理想 的 约束 力 和 力矩 与 能 量 方程 无 关 , 无 须 考虑 。 在 能 量 方程 中 , 杆 
件 的 运动 采用 独立 变量 描述 ,串联 开 环 运动 链 机 构 中 ,关节 坐标 gqg=[g， gq， … gn) 是 完 
全 独立 的 广义 坐标 ,唯一 描述 了 杆 件 系统 的 位 姿 。 考 虑 n 目 由 度 串 联结 构 机 械 辟 ,关节 无 摩 
所 上 且 不 考虑 杆 件 的 重力 ,关节 主动 力矩 + 和 末端 力 螺旋 w 之 间 的 解析 关系 为 

r-J'w Uu 

式 中 ,了 是 机 械 臂 雅 可 比 和 矩阵 。 

利用 虚 功 原理 证 明 公 式 (7. 11) ,考虑 各 关节 的 虚 位 移 为 6q=[6g， ôq … ôq], R 
端的 虚 位 移 为 p=[6x。 p.l] ,如 果 w 表示 末端 作用 于 环境 的 力 螺旋 , 则 作用 于 环境 的 虚 
功 为 w 5p ,关节 主动 力 在 关节 空间 做 的 虚 功 为 + 6g。 当 机 械 壁 在 平衡 状态 ,依据 虚 功 原理 ， 
得 


T ôq=w"ôp (7. 12a) 
虚 位 移 5p ,6q 之 间 的 约束 关系 可 以 通过 雅 可 比 和 矩阵 确定 为 
ôp =Jôq UT. 125) 


将 式 (7. 12b) 代 人 式 (7. 12a) ,进一步 可 以 写 为 
(T —w'J)8q-20 
关节 虚 位 移 6g 是 独立 变量 ,因为 开 链 的 关节 坐标 是 完整 和 独立 的 广义 坐标 ,因此 ,上 式 
成 立 只 能 有 
T —w J=0 
两 侧 转 置 可 以 得 
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T=J'w (7. 12c) 

【 例 7.2】 图 7.3 展示 了 2R 平 面 机 械 辟 ,在 惯性 参考 坐标 系 |1|} 下 ,其 末端 施加 于 外 

部 环境 的 作用 力 为 w=[f，f,] , 试 求 此 时 执行 器 输入 的 关节 力矩 +=[7， m] ,假设 没有 
摩擦 ,不 考虑 杆 的 重力 ,得 到 的 结果 与 公式 (7. 10d) 比较 ,说 明 差 异 。 

参考 式 (4. 41) 和 式 (7. 11) ,作用 关节 上 的 力 7 与 末端 作用 于 外 部 环境 的 力 w 的 关系 为 


Ó ]- l -(a,s, *a55,) DM P. (7.13) 
—058$15 C2C12 y 


比较 式 (7. 10d) 5X C7. 13) 可 以 看 出 , 式 (7. 13) 不 包含 重力 项 , 男 外 ,末端 作用 于 外 部 
环境 的 作用 力 w 在 式 (7. 10d) 中 表示 在 坐标 系 |31 下 ,而 在 式 (7. 13) 中 表示 在 坐标 系 |1| 
Fe 


7.2 牛顿 - 欧 拉 方 程 


设 刚体 的 质量 为 m, 在 惯性 参考 坐标 系 下 的 惯量 矩阵 为 1, 受 到 一 个 作用 于 质心 的 力 F 
和 力矩 NN, 首先 ,根据 牛顿 第 二 运动 规律 得 


ma zc (7. 14) 
应 用 附录 C 式 (4) ,得 
人 (1w)= lotoxlo=N CE. 15] 
简单 证 明 式 (7. 15) ,参考 附录 C 公式 (17 ) 可知 
I-RI,.R' 


AP Ie 表示 在 刚体 质心 点 C 处 建立 与 刚体 固 连 的 坐标 系 {C} ,相对 于 C 点 ,在 坐标 系 |C| 
下 计算 得 到 的 转动 惯量 矩阵 ,为 常量 矩阵 。 式 (7. 15 ) 对 时 间 微 分 ,得 


人 (1mw)= Ico*lo (7. 16) 
惯量 矩阵 对 时 间 的 微分 为 

I -RI.R' -RI.R* (7 17) 
由 式 (4. 8) 可 知 

R -.QR 
代 大 起 (7. 17) ,得 
lo- [ QRI.R' «RI, (QR)! |o- QRI.R' o - cxlo (7.18) 
将 式 (7. 18 ) 代 入 式 (7. 16 ) 可 以 得 到 式 (7. 15) 。 刚 体 的 牛顿 - 欧 拉动 力学 方程 表示 为 
ma-F 
Esel 


7.2.1 单 杆 牛顿 - 欧 拉 方程 


首先 基于 牛顿 - 欧 拉 方程 推导 杆 件 守 的 动力 学 方程 ,如 图 7. 4 所 示 ,在 惯性 参考 坐标 系 
下 ,>c 表 示 质 心 点 的 线 速度 , eo, 表示 刚体 的 角速度 ,其 他 物理 变量 意义 参考 图 7. 1 的 说 明 。 
刚体 的 惯性 力 表示 为 -myc ,其 中 m 为 杆 i 的 质量 ,vc 表示 杆 的 线 加 速度 。 基 于 达 朗 贝尔 原 
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理 , 杆 i 的 惯性 力 加 到 静 平 衡 方 程 (7. 1) 得 
fiui-fiintmg-mive,=0,i=1,.…,n (7. 19) 
作用 于 杆 i 的 惯性 力矩 为 -Tw;-w;xliw; T, 为 杆 i 的 质心 处 惯性 张 量 ,惯性 力矩 加 入 到 
式 (7.2) ,得 


Nii ihi i 一 人 Fi is Haie) Xf. it ias, Xf, ia -I;0,-c;Xl;o,20,i21,--,n (7.20) 





图 7.4 单 杆 动态 受 力 示意 图 
式 (7. 19) (7. 20) 包 含 了 杆 件 的 惯性 力 ,其 计算 涉及 机 器 人 运动 学 分 析 ,运动 学 分 析 的 
顺序 应 该 从 基 座 到 末端 ,依次 得 到 各 杆 件 的 速度 、 加 速度 进而 得 出 其 惯性 力 , 而 对 于 静 力 学 
部 分 ,为 了 求 得 各 关节 之 间 的 主动 力 、 约 束 力 ,其 计算 需要 从 末端 到 基 座 。 牛 顿 - 欧 拉 方 程 
推导 流程 如 图 7. 5 所 示 。 


速度 、 加 速 和 惯性 力 


图 7.5 牛顿 - 欧 拉 方程 推导 流程 


【 例 7.3】 图 7.6 所 示 为 1 自由 度 平面 机 械 臂 ,假定 建立 的 基 坐 标 系 为 11} ,末端 坐标 
系 为 121 (REHE) , 杆 的 长 度 为 a, 基于 牛顿 - 欧 拉 方程 ,推导 其 动力 学 方程 。 





O; 


图 7.6 平面 单 杆 牛顿 - 欧 拉动 力学 建 模 
杆 的 姿态 用 两 坐标 相对 旋转 和 矩阵 C 表示 为 
c -s 0 
s c Q0 
0 0 1] 
式 中 ,c,s 分 别 表 示 cos q,sin q o TF 1 为 匀 质 截面 为 方形 的 杆 件 , 在 C 点 建立 与 坐标 系 |2|1 


Q- 
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方向 一 致 的 坐标 系 { C} ,在 此 坐标 系 下 , 杆 件 为 小 截面 , 绕 * 轴 的 转动 惯量 忽略 不 计 , 杆 的 转 
动 惯 量 和 矩阵 为 
,|0 0 0 
0 1 1 
D MA! 


ma 
Ic-5 





矩阵 Iz 变换 到 坐标 系 {1| PON 


S -sc 0 
[Ic]: =Q nud c 1 
0 0 1 
首先 计算 杆 1 的 速度 与 加 速度 ,为 


c-r, c=r 2+0 nde +Q 


(1) 运 动 学 分 析 。 


质心 点 为 杆 的 中 心 ,其 位 置 矢量 为 





—a/2 ac 
0 = 二 as 
0 0 0 


式 中 ,rc 表示 从 坐标 系 11| 原点 01 指向 杆 1 质心 点 的 矢量 ;c 表示 杆 质 心 点 在 坐标 系 |1} 下 
(2) 建 立 牛 顿 - 欧 拉 方 程 。 


的 位 置 天 量 。 则 
0 0 
重力 加 速度 方向 在 坐标 系 |11 下 为 


g-[0 -g 0] 
假定 杆 1 对 外 部 环境 无 作用 力 , 则 
fi;2n,,20 
依据 式 (7. 19) 和 式 (7. 20) ,机 械 辟 只 有 一 个 杆 件 , 则 i=1。 此 外 ,对 于 平面 机 构 , 欧 拉 
方程 中 项 oxo 为 零 , 则 


—$ 
C 


0 


: aq 
d 








fo +mg-me =0 
noari cXfo -lL Ic] ,@=0 
进一步 计算 得 
-Fmal ds+ge) 


fT | Smal je-gs) ma 


0 
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0 


- 0 
n, 1 = 


1 5 1 
4 ma q+; mgac 


3 2 
最 后 应 用 式 (7. 8) ,计算 r, 所 得 动力 学 方程 为 


ERE GNE 
T=3 ma q+; mgac (7.21) 


在 上 述 计算 过 程 中 ,如 果 没 有 特殊 说 明 , 所 有 变量 均 在 坐标 系 11}+ 下 表示 。 单 杆 机 械 臂 
实际 绕 01 点 转动 ,其 欧 拉 方程 也 可 以 写 为 
n, tr; cXG-Io w=0 
式 中 ,G 为 杆 受 到 的 重力 ;1o ez TEASE ZR (11 下 绕 O 点 转动 的 惯性 矩阵 。 
【 例 7.4】 参考 图 7. 3 ,在 惯性 坐标 系 下 ,应 用 牛顿 - 欧 拉 法 ,推导 2R 机 械 臂 动力 学 方 
程 ,图 中 ac ,ac, 为 杆 1、 杆 2 离 原点 01,0, 的 距离 。 坐 标 系 之 间 的 旋转 矩阵 为 











&' x Q 
2 c 0|,i=1,2 
0 0 il 
FF IF 2 各 上 自 绕 质心 转动 ,在 坐标 系 121 ,131} 下 的 惯性 矩阵 为 
JO O Q 
天 = 一 o 1 0|,i=1,2 
i7 12 , 
0 0 1 
(1) 运 动 学 分 析 。 
FF 1 的 质心 点 C, 在 坐标 系 |11 下 的 位 置 矢量 c, 为 
€y 77,6 7| 463; 
0 








FF 2 的 质心 点 C, EARR 下 的 位 置 矢量 c, 为 


C2 一 7T2T72D 











04€, dc, Cc2Cl2 
AUT Vf uc ası ,F;c,7 Q0, [7;.,15 2 Q.Q, 0 |=| ac s2 
0 0 0 
则 


QC, td, cl 
€;—|0,5, 06,51 
0 


FF 1 角速度 co, 与 加 速度 eo, 与 质心 点 线 速度 c, 与 线 加速 e, 为 


0 0 
w =| 0 | ,mw,=|0 
qi qi 
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—$, —$, é 
P dul C ,6 26d d, C] -àc di $1 
0 0 0 
FF 2 角速度 co, 与 加 速度 o, 与 质心 点 线 速 度 c, 与 线 加 速 c, 为 
0 0 
wo | O0 ,机 =| 0 
qı +q qi +q 
-a,5,4 -ac Snn -a15191-Q1c191—( ac, Snn tac, C151 ) 
e; = aic1q1+ac,c12912 €; a 0,€,Q; 0,5, Q1 * (0c,01,15 00,5121; ) 
0 0 


(2) 建 立 牛 顿 - 欧 拉 方 程 。 
在 惯性 参考 系 |1| 下 ,对 杆 2 运用 牛顿 - 欧 拉 方 程 得 
fiz +m,8-m,C, =0 
ni 27r c, xf, ;- Le, -0 
经 过 计算 得 
m; —a,5,4, -a,0,4; -( Gg, S151 +ac,ci2g12 
fi2=| m[a,64, -a1s1g91+( TAE A A ) ]+m,g 
0 
0 
ii sc 0 
maqa * m3, q, *v, +6, 
对 杆 1 运用 牛顿 - 欧 拉 方 程 得 
fo, fi 2+m,g-m,c, -0 
No , 7T, 5—T, c, Xfo | T» c, Xf, ,-I, o, -0 
计算 得 
0 
PLE 0 
m,, q, +M q2 —q; -2vq,q, +G; 
式 中 
m,,-m, ac. +I 4m,( a; ác, +2alawc, ) +, 
ni, = -, 
Ins — 5 fh. ác, *a,0,,C; ) +I, 
v-m»;d,d,s; 
Ci 2m,ga,, c, *m;g( a.c, *a,c, ) 
G, =M2£0c,C12 


2R 机 械 臂 动力 学 方程 写成 矩阵 形式 为 
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图 [7 "E Ja [7 Prag inj (1,22) 
T? T, G, 


7.2.2 动力 学 方程 的 物理 意义 


阐述 平面 2R 机 械 辟 动力 学 方程 所 蕴含 的 物理 意义 ,方程 中 最 后 一 项 G1,G,, 是 由 重力 
引起 的 ,为 杆 1 和 杆 2 的 质量 作用 与 关节 1 和 关节 2 的 重力 矩 , 当 机 械 臂 水 平展 开 时 ,重力 
ERK 

动力 学 方程 (7. 22) 中 第 1 项 m 为 惯性 项 。 当 第 2 个 关节 保持 不 动 , 即 g =q 20 , 2L 
重力 项 ,动力 学 方程 为 r 2 mq qi ,表示 为 杆 1、 杆 2 一 起 加 速 运动 作用 于 关节 的 力矩 。ml 表 
示 关 节 1 转动 时 , 杆 1 和 杆 2 的 转动 惯量 ,其 中 ,miac HI, 表示 杆 1 绕 关节 1 的 转动 惯量 , 另 
外 一 部 分 与 杆 2 有 关 , 在 当前 状态 下 ,如 图 7.7 所 示 , 杆 2 的 质心 点 与 坐标 系 |11} 原点 的 距离 
为 


hg b. 
L za,*a,;,*2a,a,,c; 





7.7 2R 机 械 臂 动力 学 方程 各 项 物理 意义 
根据 平行 轴 原 理 , 杆 2 绕 关 节 1 的 转动 惯量 为 mL + ,依赖 于 机 械 臂 的 位 形 , 当 q, =0 
时 ,最 大 ; 当 9: = 站 时 ,最 小 。 
第 2 项 ma 也 为 惯性 项 , 当 杆 1 保持 不 动 , 即 q =q, =0, E q,=0 时 ,忽略 重力 项 ,可 以 得 
到 7, =miz9: ,来 自 关 节 2 的 加 速 运动 ,此 时 为 关节 2 做 加 速 运动 所 产生 的 力作 用 于 关节 1， 
FF 1 瞬时 静止 ,根据 式 (7.4) 得 


no1=n +71, Xfi, (7. 23a) 
FF 2 加 速 运动 ,其 动力 学 方程 为 
fi; 7m;c; 
B 2f, o Xf SHE, 0, (7. 23b) 
将 式 (7. 23b) 代 入 式 (7. 23a) ,得 
0 
0 d; 








m,( ac, +taiac,c ) +I, 
第 2 项 表示 杆 2 的 加 速 运动 对 关节 工 产 生 一 个 反作用 力矩 ,关节 1 需要 额外 驱动 力 甜 
Ti 平衡 。 
第 3 项 vq 中 4v 乘 以 关节 速度 的 平方 ,假定 在 瞬时 丁 2 保持 不 动 ,q,=9,=0, 91=0, 关 
T1 做 匀速 转动 ,如 图 7.7 所 示 , 在 杆 2 质心 处 产生 离心 力 , 此 离心 力 的 大 小 为 
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j JAN amli 
此 离心 力 在 关节 2 产生 一 个 力矩 ,为 
0 
0 


"2 
—m;,0,$5,q|; 


同 理 , 杆 2 做 匀速 转动 引起 的 离心 力 ,作用 于 关节 1 处 的 力矩 为 vq ,需要 施加 的 反作用 
力矩 为 -gz。 

方程 中 第 四 项 -2vq1g, ,在 瞬时 大 小 正比 于 关节 速度 的 乘积 。 瞬 时 关节 1 和 关节 2 以 速 
度 qi 和 q 运动 ,建立 一 个 临时 坐标 系 O6-xzsysz ,其 原点 与 坐标 系 12| 原点 重合 ,瞬时 方向 
与 坐标 系 {11} 一 致 ,坐标 系 转动 角速度 为 9, ,在 此 坐标 系 下 , 杆 2 质心 线 速度 为 cq ,参考 公 
式 (4.28) ,由 科 氏 加 速度 引起 的 科 式 惯性 力 为 


Teen =r 6 xf... - 








fco, 7» -2mj(o, xv,)- 2m,ac,q9192512 (7. 23d) 


2m,ac, gi ^ 
0 





科 氏 力 在 关 1 引起 的 力矩 为 


0 
Tcu 7T c Xf cor = 0 
2m;a; Qc, 01023» 


因为 科 氏 力 如 式 (7. 23) Bios ,平行 于 杆 2, 所 以 在 关节 2 处 不 会 引起 力矩 。 
7.2.3 机 器 人 动力 学 方程 的 结构 


当 针对 不 同 的 机 械 臂 ,利用 不 同 的 动力 学 建 模 方法 得 出 其 动力 学 方程 后 ,为 了 分 析 方 
便 , 可 以 整理 成 不 同 的 形式 。 
1. 状态 空间 方程 
可 以 把 得 到 的 动力 学 方程 写成 
r=M(g)g+V(g,q)+G(g) (7. 24) 
式 中 ,M 为 维 数 nxn 机械 臂 惯性 矩阵 ;V 为 维 数 nxl 矢量 ,包含 离心 力 和 科 氏 力 项 ;G 为 维 
数 nxl 矢量 ,包含 重力 项 。 把 形 如 (7. 24) 的 方程 称 为 状态 空间 方程 ,因为 式 中 V(q,q) 项 依 
赖 于 广义 坐标 的 位 置 和 速度 ,而 M,G 项 依赖 于 广义 坐标 。 
2. 位 形 空间 方程 
通过 把 V(q ,gq) 项 分 解 ,进一步 写成 
r-M(q)q*B(q)[qa]*C(a) [d ]+G(g) (7.28) 
式 中 ,B(9) 是 维 数 nxn(n-1)72 科 氏 系数 矩阵 ; [ gq] 为 维 数 n(n-1)/2xl 关节 速率 相 乘 矢 
量 为 
[44] = [qia， 9193 2 ET A 
C(g) 是 维 数 nxn 离心 力 系数 矩阵 ; e] 为 维 数 nxl 矢量 为 
[d le[di. d * dl 
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式 (7. 25 ) 称 为 位 形 空间 方程 , 式 中 矩阵 M,B,C 和 G 全 部 依赖 于 广义 坐标 qo WRZ 
力学 方程 中 各 项 应 用 于 机 械 臂 的 控制 中 , 随 着 机 械 臂 的 运动 ,其 动力 学 方程 中 所 有 怎 阵 的 计 
算 仅 依赖 于 位 置 的 变化 。 


7.2.4 机 器 人 系统 计算 仿真 


前 面 讲 过 ,机 器 人 动力 学 通常 涉及 两 个 主要 问题 :一 个 称 为 逆 动 力学 问题 ,在 给 定 机 器 
人 几何 参数 惯性 参数 和 关节 运动 ,包括 位 置 、. 速度 及 加 速度 前 提 下 ,计算 关节 的 力 或 力矩 ; 
另外 一 个 问题 为 正 动力 学 问题 ,给 定 机 器 人 几何 参数 .惯性 参数 以 及 关节 驱动 的 力 或 力矩 ， 
计算 关节 运动 。 道 动力 学 用 于 机 器 人 控制 中 ,例如 ,动力 学 建 模 中 惯 项 、 离 心力 项 及 重力 项 
用 于 控制 相关 的 补偿 , 正 动 力学 应 用 于 控制 方法 的 仿真 ,控制 方法 在 用 于 机 需 人 实际 系统 中 
之 前 ,计算 仿真 是 一 个 重要 的 过 程 。 
当 针 对 具体 的 机 器 人 对 象 进 行动 力学 建 模 时 ,所 建立 的 动力 学 方程 写 为 式 (7. 24) 的 形 
式 , 即 
r=M(q)q+V(q,q)+G(g) 
对 于 机 器 人 运动 系统 ,定义 状态 矢量 为 x=[g gq] 包含 机 器 人 系统 的 广义 坐标 与 广义 
速度 ,x=[g 9]7, 上 式 可 以 写成 非 线性 状态 空间 的 形式 为 
xzf(x,u,t) (7.26) 
式 中 ,uw 为 系统 输入 ,表示 作用 于 广义 坐标 的 广义 驱动 的 力 或 力矩 。 对 x 的 求解 必须 满足 初 
始 条 件 x(to)=xo。 式 (7.26) 的 目的 是 把 式 (7. 24) 微 分 二 阶 方程 降 为 一 阶 方程 。 式 (7. 24) 
可 以 写成 式 (7. 26) 的 隐形 式 , 即 


- H "Lidia! 4] (7.27) 


式 中 ,7 为 作用 于 广义 坐标 的 力 或 力矩 ,是 系统 的 输入 ,可 以 表示 为 时 间 的 函数 ;7 为 单位 矩 
阵 方 阵 , 维 度 与 4g 一 致 。 在 大 多 数 情况 下 , 式 (7. 24) 为 高 度 非 线 性 微分 方程 ,在 一 般 情况 
下 ,在 给 定 7 的 前 提 下 ,得 不 到 g 的 解析 解 ,需要 通过 数值 积分 的 方法 求 4 R do HFR 
(7.26) ,给 定 x,u,t 计算 x 的 过 程 ,是 一 个 函数 评价 过 程 (Function Evaluations) ,所 计算 的 加 
速度 是 位 置 和 速度 的 函数 。 通 过 数值 积分 的 方法 ,在 离 算 的 时 间 4 ,is,,… ,i 处 求解 x, 需 要 
定义 一 个 时 间 步 长 At 1, -i;, 通 党 此 步 长 在 数值 积分 过 程 中 为 津 量 ,而 在 数字 化 运动 控制 
系统 中 ,控制 信号 也 以 离散 的 时 间 间 隔 更 新 ,因此 可 以 通过 数值 模拟 的 方法 ,对 机 器 人 控制 
系统 进行 仿真 。 单 输入 单 输出 数字 化 闭环 控制 方案 如 图 7. 8 所 示 。 


À L Da G f) ( Yı 
2.7 59^ [eun [nsa "^ [ xu 9? 


图 7.8 单 输入 输出 数字 化 控制 系统 原理 图 
图 7.8 中 ,r(z) 为 期 望 的 连续 输入 轨迹 ;y(t) 为 跟 踩 输出 ;7 为 采样 周期 。 在 第 大 个 采样 
周期 中 ,期 望 轨迹 信号 为 7, 反馈 信号 为 7 ,产生 的 输入 控制 器 的 信号 为 ei ,控制 右 经 过 计算 
并 通过 执行 器 作用 于 系统 的 输入 为 u, ,在 保持 器 作用 下 ,在 一 个 采样 周期 内 ,作用 于 系统 的 
答 入 保持 不 变 ,系统 的 输入 输出 是 连续 的 ,因此 可 以 通过 数值 的 方法 ,对 机 器 人 系统 进行 运 
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动 控制 方法 的 仿真 。 假 定 系 统 的 模型 为 机 器 人 动力 学 模型 ,其 单个 坐标 输入 力 或 力矩 r, 即 
为 图 7. 8 中 运动 控制 的 输入 4, 在 离 算 系 统 下 从 kT 到 (k+1)7 周期 内 ,对 zir 的 积分 可 以 表 
示 为 
X,rD. 1. X(t1)T 
RH n. i. s 表示 数值 积分 子 程 序 ( Numerical Integration Subroutine) , 利用 一 阶 积 分 方法 , 正 
动力 学 计算 仿真 过 程 见 表 7. 2。 
表 7.2 正 动力 学 计算 仿真 过 程 


步骤 1: 给 定 采 样 周 期 了, 仿真 结束 时 间 i JF AR BE IBI EE DJ t-kT(E 20,1,2, 2, 45 4E xir = 
[ dur W]e 

步骤 2: 可 依据 给 定 的 控制 方法 ,计算 出 上 时 间 输 入 的 力 或 力矩 7。 

步骤 3: 利 用 式 (7. 27) ,计算 得 出 xir, 此 过 程 用 限 数 评价 表示 为 E 人 k Tiat) tz kT, Xir = [os 
Bal 已 计算 得 出 ,作为 输入 变量 输入 数值 积分 子 程 序 , 获 得 x(i,1)r=[ gr al s 

步骤 4:t=t+7, 判 断 是 否 tena ,和 右 是 ,结束 ;否则 转 到 步骤 2。 


数值 积分 子 程序 可 采用 Runge-Kutta 四 阶 积分 法 (简称 R-K 法 ) , Adams, Gear 等 积分 
方法 ,不 同 的 积分 方法 ,步骤 3,4 会 略 有 不 同 。 

[517.5] 图 7.6 所 示 为 单 杆 机 械 臂 , 杆 长 为 wa=1 m, 质 量 为 m=1 kg, 重 力 加 速度 为 
g=9. 81 m/s 。 初 始 条 件 为 1=0,q(0)= 0/18 rad,q(0) 2 0。 在 机 械 臂 在 力 的 作用 下 从 初始 
位 置 到 期 望 位 置 q=7/3 rad ,gt=0。 作 用 于 关节 的 驱动 力矩 表示 为 7=7e, 其 中 7 表示 作用 
关节 上 力矩 的 大 小 ,e 为 关节 轴线 的 方向 。 其 控制 系统 的 采用 PD 控制 方法 表达 为 


r2 y (470) -B(9) 7 mgac 
式 中 ,常量 增益 为 y=30 N - m/rad, 8-45 N . m © s/rad. 4j Matlab 程序 仿真 ,在 此 控制 方 
法 下 , 单 杆 机 械 臂 的 关节 实时 输出 位 置 9 和 速度 4。 首 先 定义 系统 的 状态 矢量 为 
X-[s g]'xs[g 4l" 
根据 式 (7. 27) ,关节 加 速度 了 可 以 表示 为 
“ 3 l 
g= = r-z mgac| 
写成 状态 空间 的 形式 为 


二 
| (1.28) 


x(2)- d r- mgac) 
ma 


HITELA RR EH — DT PALA 73 Tg fi EEN — DT 10 4T 73 FE , Matlab 程序 见 附录 D。 位 
置 、. 速 度 曲线 仿真 图 如 图 7. 9 所 示 。 
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7.9 单 杆 机 械 臂 的 PD 控制 仿真 


7.2.5 递归 牛顿 - 欧 拉 算 法 

假定 研究 的 串联 机 械 臂 包含 基 座 总 共有 nel 杆 ,m 个 关节 ,关节 类 型 为 转动 关节 或 者 移 
动 关节 ,已 知 机 械 辟 各 物理 参数 ,关节 位 置 .速度 和 加 速度 分 别 为 gq,qg 和 4, 采用 递归 牛顿 - 
欧 拉 算 法 求解 此 机 械 臂 逆 动 力学 过 程 如 下 。 

1. 运动 学 的 外 推 

串联 机 械 臂 的 串联 结构 参见 图 4. 10 ,在 基 座 标 系 |1| 下, 杆 i 的 角速度 o, MEF i71 的 角 
速度 co, | 的 弟 推 关系 见 式 (4. 43)、(4. 44) , 即 


: wite 第 i 个 关节 为 转动 关节 (7.29) 
w=] eu. 第 i 个 关节 为 移动 关节 | 
角 加 速度 递 推 关系 为 
€, ， 十 COi_1 Xq;e,*q;e; 
osi (7.30) 
€); , 


式 中 ,i=1,2,…n;wo 和 coy 是 基 杆 的 角速度 与 角 加 速度 。 式 (7. 29) (7. 30) 是 坐标 不 变 公 
式 , 即 在 任何 坐标 系 下 都 成 立 , 只 要 公式 里 所 有 变量 表示 在 同一 个 坐标 系 下 。 为 了 降低 数值 
计算 的 复杂 度 ,第 i 个 杆 相 关 的 所 有 的 矢量 或 矩阵 变量 表示 在 坐标 系 Faa F Le; 在 坐标 系 F, 
下 的 表示 非常 简单 ,为 [0 0 1]", 看 作 i-1 根 杆 上 的 矢量 。 在 坐标 系 尺 下 的 矢量 和 矩阵 
变化 到 坐标 Fu IRI ,需要 左 乘 旋转 矩阵 CO: 。 因 此 角速度 o, 和 角 加 速度 c, 在 坐标 系 Fa 
下 的 计算 递 推 关 系 为 
[CCw-+giei) 
"lata. 
. [Q1 e, , c, ,Xq;e;*q,e;) 
aes (7.31) 
lata. 
如 果 基 座 固定 不 动 , 则 eo, — 0 , eo — 0 , YEAR ER ZR F, F ,在 式 (7.31) 中 ,各 变量 为 
[e;];=[0 0 1]' 
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Allwn] lo w, w.] , 则 














w, 0 
[w;1xq;e;]:=4q;:| ^o, | ,[qiei];i=| 0 
0 qi 
当 在 坐标 系 Fi 下 给 定 w; 1 时 ,可 以 在 坐标 系 Fia PRIR w;。 进 一 步 , 令 c; 为 杆 i 的 
质心 点 C; 的 位 置 矢量 ,rc 表示 从 Fi 原点 0; 指向 C; 的 矢量 ,从 图 7. 10 可 以 看 出 两 个 连续 
杆 的 质心 之 间 的 位 置 关系 为 
C; =C; Sh a EE A ) 了 (7.32) 
式 中 ,对 于 转动 关节 ,Tr;1,; ,rc, 在 坐标 系 F; 下 为 常量 , 同 理 ,r, EAS RAS Fa PUE ER 


第 i 杆 件 





图 7.10 R 关 节 相 邻 两 杆 位 置 关系 
如 果 第 i 个 关节 为 移动 关节 ,如 图 7. 11 所 示 ,两 个 连续 杆 的 质心 的 位 置 关 系 为 





图 7. 11 了 关节 相 邻 两 杆 位 置 关系 


C=C Hirn Taat ) +d;e;+F o c, =C; +7c id+Giei+riao c. (7.39) 
式 中 ,di; 为 关节 变量 ,rc 表示 1d;|=0 时 ,原点 0; 指向 杆 i 质 心 点 的 矢量 ,在 坐标 系 已 ,下 


也 为 常量 。 式 (7.32)、(7. 33 ) 两 侧 对 时 间 微 分 , 则 杆 i-1 和 杆 i 线 速度 和 线 加 速度 之 间 的 关 
AUF: 
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如 果 第 i 关节 为 转动 关 市 
C; Cj , 60; 1 XP. | ‚tO; ^F; e, (7 34) 
C; 2€, 4*0); Xc, , t€); X (0, XT c, i) + XT c, «X (ex; XT: c.) 


如 果 第 i 个 关节 为 移动 关 市 
;= ;1 


H;-TC. itdie;+rio,c, 


"HM (7.35) 
C; 2C; , «0, XU *d;€; 


€; -6., +0; XU; +0;X( e, xz .+2d e， ) +de, 
式 中 ,i=1,2,…n,co,co 是 基 杆 的 质心 速度 和 加 速度 ,在 基 坐 标 系 下 为 
c, 20,c, -0 

3&7. 34) (7. 35) 为 坐标 不 变 公式 ,在 任何 坐标 下 都 成 立 。 因 此 为 了 简化 计算 ,e,， 
re; 和 在 与 杆 i71 固 连坐 标 系 F, M r, c TEST i 固 连坐 标 系 ,下 为 常量 ,因此 式 (7. 34)、 
(7.35) 可 以 表示 在 与 杆 ; 固 连 坐标 系 已, 下 ,mw; ,mw;,ci,c; 迭代 计算 中 在 坐标 系 F, 下 的 矢量 
经 过 坐标 变换 到 坐标 系 Faa 下 ,与 在 坐标 系 Fa 下 表示 的 矢量 相 加 。 运 动 学 外 推 计算 见 表 
Js a 
7.3 运动 学 外 推 计 算 


% 假 定 机 器 人 系统 有 nt 个 关节 ,n+1l 根 杆 件 , 所 建立 坐标 系 为 ntl 个 ,符号 为 Fi SF, 
% 给 定 以 下 初始 值 
€), ,Co sO ,C0 ,C0 
% 建 立 坐 标 系 ,D-H 参数 表 , 已 知 关 节 位 置 速度 和 加 速度 为 gq,q 和 9, 在 所 建立 的 坐标 系 下 计算 得 出 
以 下 常量 : 
ri, adio Dre da s£m1,2, 7, 
2o 计算 当前 位 姿 下 的 旋转 矩阵 
Q, - Q, 
和 开始 往外 递 推 
for i = 1 ton step -1 
7o 更 新 
Q, 
if R XT then 
[w] aQ; Le, *4;6;]; 
[€], 0; [e xdgiei+giei] 
[e 1240 Lezione, idit rud 
Le; J] 9; [c, to Xre, ilit EDX, e, lia 
[e] 0 [e+eo Xro 009a X( wi Xre, i) Ji+[ c, xr, c, *€X(eXr; c.) Jis 
else 
€; =; 
Le.) Le. ]; 
[1.97 Le. ], 
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[ u;] ii€ Qi [re itdie, ] itl Toc, li 
Le; ] ic Qi [6.4 *d;e, ] ;+[ «xu, lia 


[6, ] iai [6;. *d,e;*«,X2d,e, ] ;十 [ €; XU; 十 COiX( c; XU; ) dii 


end if 
end do 
2. 重力 加 速度 外 推 
重力 加 速度 从 坐标 系 ,变换 到 坐标 系 已 ,的 递 推 公式 为 
[ela = [g];,4£21,2,--,n (7. 36) 
3. 动力 学 计算 内 推 


以 末端 杆 , 即 第 n 根 杆 件 为 例 , 分 析 其 受 力 情况 , 杆 n 所 受 的 力 如 图 7. 12 所 示 , 惯 性 力 
未 表示 出 。 图 中 ,fn ,n,mn 表 示 机 胡 人 末端 工具 对 外 部 环境 施加 的 作用 力 和 力矩 ,如 果 外 
部 环境 对 末 问 施 加 的 力 和 力矩 为 1 和 ,根据 作用 力 与 反作用 力 f, uam fona no TE 
式 (7. 19) 〈7. 20) ,未 闯 的 杆 牛顿 - 欧 拉 方 程 写成 递 推 的 形式 为 





图 7. 12 ”末端 杆 受 力 分 析 


eia lur Dont «m,(c,-g) Loi 
EY uut E Wera) Wt HIS HL c, +0, XL, c, ] ,1 7.37) 
如 果 第 n AX 2 ESI T , DU 
[nain] =Q, Eu dun 


(7. 38a) 
n= le ; Lib sal. 
如 果 第 个 关节 为 移动 关节 , 则 
a E ud (7. 38b) 


sel. LE LL 
牛顿 - 欧 拉 方 程 对 其 余 杆 件 的 递 推 关系 为 
[Siiil = [fiitmi( ci-g) | 
p TF lui p Te HOP Hiag Xfi, i Tae; Xf iis Hc, o), XI e, | ii (2.39) 
动力 学 内 推 计算 过 程 见 表 7. 4。 
表 7.4 动力 学 内 推 计算 过 程 


给 定 fani sh n+l 
计算 出 Q, ~Q, ,给 定 [g], ,计算 [g], ~[g],, 


for i=n to 1 step -1 do 
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PPP ] pr [fuss «mj(€,-g) la 
[ni d Jia? LTE t(rLui Fui) Xf V i7 Tia c Xfi in tete Xw; liga 
% 在 坐标 系 il 下 求 得 LA ilia Unoulua ,转化 到 坐标 系 i 下 的 方程 为 
Li 一， [fiii lin 
[ri liQ; Ln; i 
if R 关节 then 
Title]: * lnis]: 
else 
r,—l e;]; * faul; 
end if 
end do 


7.3 拉 格 明日 方程 


7.3.1 第 一 类 拉 格 朗 日 方程 


在 惯性 负 卡 儿 坐 标 系 下 ,考虑 一 个 包含 m 个 质点 的 系统 15} = {P,P,,…,P,| 的 运动 ， 
ry Tyr ,Tm 表示 各 质心 点 的 位 置 撩 量 ,第 i 个 质点 的 动力 学 方程 为 
F,-m,a, (7. 40a) 
式 中 ,F; 表示 作用 于 质点 已 所 有 主动 力 的 合力 ;m; 为 质点 的 质量 ;a; 为 质点 在 惯性 系 下 的 
加 速度 。 





图 7.13 m 质点 系统 


质点 已 的 惯性 力 定义 为 


F,;7-m,à, (7. 40b) 
根据 达 明 贝尔 原理 , 式 (7.40a) ,(7. 40b) 可 以 写 为 
F;-F,,-0 (7. 41) 


令 质心 系 的 各 质心 坐标 为 9=[g， qd; … Q4.) ,在 惯性 笛 卡 儿 坐 标 系 下 ,gq 表示 3m 
个 相互 依赖 的 坐标 系 ,假设 * 表示 系统 独立 约束 方程 的 个 数 , 则 f=3m-s 表示 系统 的 自由 度 
数 。 其 约束 方程 可 以 表示 为 
(q,1)-0 (7.42) 
AX'P,6-(0,,0,, 0.) , 0, 表示 第 i 个 标量 约束 方程 。 对 式 (7. 42) 两 边 取 变 分 ,得 
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m 





Y B S 0, b e 1,3. (7.43) 
SLA HR BH H3 T ACE = L3 5,5) , 式 (7.43 ) 两 边 乘 以 和 A;, 并 对 上 求 和 ,得 
Zaa < 名 a)i 3 (Za) ôq, = 0 (7.44) 


根据 达 明 贝尔 - BALL OR C Je 过 方程 ) ,在 理想 约束 条 件 下 ,质心 系 任 意 瞬 
时 ,所 受 主动 力 系 和 虚 加 惯性 力 系 在 虚 位 移 所 做 功 的 和 等 于 零 , 则 系统 1 3 | 动力 学 方程 为 





是 FL) “ 64;=0 (7.45) 
式 (7.45) 和 式 (7.44) 相 减 ,得 
X (F; - mä - PA) = (7.46) 


对 于 s 个 不 独立 的 坐标 变 分 ,可 以 选取 适当 的 A, ,使 变 分 前 系数 为 零 ,独立 坐标 变 分 前 

的 系数 也 应 等 于 零 。 写 为 矩阵 形式 为 
Mq«C'A-F (7.47) 
XP, M -diag( m, ,m,,--- ,m,) e R”" 为 惯性 矩阵 ,其 中 ,m,=diag(m,,m,,m,) e R, A= 


[À; dg cU alen, A (7. 46 ) 称 为 第 一 类 拉 格 明日 方程 ,方程 中 共有 3m+s 个 未 知 


量 ,4 中 3m 个 元 素 与 入 BP s 个 元 素 , 须 与 方程 (7. 42) 联 合 求解 。 式 (7. 42 ) 对 时 间 两 次 微分 
得 
Cq=-®,-Ċq=y (7.48) 
式 中 ,更 表示 D 对 时 间 两 次 微分 。 
式 (7. 47) 和 式 (7. 48) 写成 矩阵 形式 为 


& ella "in Bot 


XC. 49) E à 3m+s dab Mic 3m«s 个 未 知 量 。 甜 阵 通 常 非 正定 ,在 实际 
系统 中 可 能 稀疏 。 利 用 式 (7. 49) ,如 果 M 非 奇 异 ,可 以 获得 瞬时 加 速度 g 和 拉 格 朗 日 乘 子 
A 的 表达 式 为 

q-M'(F-C'A) (7. 50) 
将 式 (7.50) 代 入 式 (7. 48) ,得 
CHM  C A=CM 'F-y 
可 以 求解 出 入, 进而 计算 出 加 速度 qr 

第 一 类 拉 格 朗 日 方程 , 既 适 用 于 完整 约束 ,也 适用 于 非 完 整 约束 。 它 包含 了 力学 系统 的 
全 部 问题 ,可 求 出 系统 的 运动 ,也 可 求 出 各 约束 系统 在 系统 中 的 约束 力 。 下 面 推导 第 二 类 拉 
格 表 日 方程 是 第 一 类 的 一 部 分 。 

【 例 7.6】 图 7. 14 所 示 为 平面 2R 机 械 臂 ,在 参考 坐标 系 |1|} 下 (其 他 与 杆 件 固 连坐 标 
系 未 画 出 ) , 杆 的 长 度 为 a ,az ,质量 为 m, ,m,。 各 杆 质 心 点 C1,C, 分 别离 原点 0, ,0, 的 距 
FJ ac, ,ac , 绕 质 心 的 沿 与 z, 轴 平行 方向 的 转动 惯量 为 1 ,1,。 对 于 第 i 杆 件 ,用 参数 x,,y， 
和 q; 来 表示 质心 ,9， -[x; yi q;] 的 位 置 和 杆 的 姿态 ,其 系统 的 坐标 可 以 定义 为 q = 
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[x yi 4 X » 4 ] ,惯性 矩阵 为 M-[m, ma" f, .5, f, L] ÆJ F= 


[0 -mg 7Ti 0 -mg Pel a 
参考 图 7. 14 和 式 (1.5) ,得 到 四 个 约束 方程 为 


X; 70,€| 
bia y179631 
q = = 
| x, * (a,-a6,) c; | -(x,7a6,01;) 
[ y; tlar ae, )5, | -(y2786,515) 
1 0 Qc, $; 0 0 0 
c 90. 0 1 74, C 0 0 0 
oq 1 0 -(a,-a,,)5, 786,51 e oW 706,512 
0 1 (a,-a4)c6*ta86,06; 0 -1 dep 
F 
~ac lfi 
.2 
M. ux ~ac s14 
y=-®,-Cq = 


$7 . LI 2 
| (a, -ac ) C1+ac, ci Jaqi +2a¢c,c129192+ac,c1292 
"2 . . Kr 
[ (a, -ac ) Si *G, $15 lai +2ac,5129192+Q0,51292 


使 用 定义 的 gq,M,F,C 和 YY, 可 以 建立 平面 2R 机 械 臂 的 动力 学 方程 。 





A; Guy) 
| 


Àj 


7.14 拉 格 明日 乘 子 作用 示意 图 


7.3.2 第 二 类 拉 格 朗 日 方程 


[1531] 


(7.52) 


(7:33) 


牛顿 - 欧 拉动 力学 方程 基于 牛顿 第 二 运动 法 则 推导 ,所 建立 的 方程 中 包含 了 约束 力 ,为 
了 获得 解析 的 动力 学 方程 ,在 计算 过 程 中 需要 排除 掉 理想 约束 力 , 计 算 过 程 复杂 ,物理 意义 
直观 。 第 二 类 拉 格 朗 日 方程 基于 能 量 和 功 , 而 非 作 用 于 单个 刚体 上 的 力 和 力矩 ,推导 过 程 规 
范 而 不 易 出 错 , 是 多 目 由 度 系统 动力 学 问题 重要 的 理论 基础 ,并 能 应 用 于 柔 体 或 刚 - 柔 耦合 
系统 。 本 小 节 首 先 论述 第 二 类 拉 格 朗 日 推导 的 过 程 ,明确 其 中 变换 和 演绎 的 物理 意义 。 
如 果 系 统 1$} 是 一 个 完整 约束 系统 ,如 图 7.13 所 示 , 具 有 个 自由 度 , 则 在 惯性 参考 坐 


标 系 下 ,质点 P WARE r, 可 以 表示 成 n 个 广义 坐标 gqg=[g， y 
量 函 数 为 
r;-Tí(q1,95,*7*,0,,t) 


q,] 和 时 间 的 矢 


(7. 54) 
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在 参考 坐标 系 下 ,对 公式 (7. 54) 求 微分 ,质点 P; 的 速度 表达 形式 为 
yo PAPA (7.55) 
Bs ðq; ðt ot 
重 写 为 
»- Y 00d € (T. 56) 
式 中 ,(v;); 称 为 质点 P; WE kx RE ( Partial Velocity) ,定义 为 
TUNE (7. 57) 
0q, 


矢量 r, 是 广义 坐标 4 和 时 间 的 函数 ,与 广义 速度 g 无关, 而 广义 坐标 4 中 各 变量 相互 独 
立 , 式 (7.56) 对 g 求 导 ,可 以 得 出 一 个 等 效 变 换 


(7. 58) 
dde  0q, 
由 虚 位 移 的 定义 ,对 六 变 分 运算 ,得 
gr = Y, (48, (7.59) 
式 中 ,59, 为 广义 坐标 qu 的 变 分 , 称 为 广义 虚 位 移 。 根 据 动力 学 普遍 方程 ,得 
= (7.60) 
R(T. 60) 交换 i 和 上 的 天 加 顺序 
Een : (v),] dg =0 (7.61) 


XC. 61) 中 , 方 括号 中 的 物理 量 与 69; 之 乘积 为 功 ,因而 其 物理 意义 对 应 广义 坐标 gi 的 
] XJ] ,将 含 主动 力 F, 和 惯性 力 Fai 的 项 重新 定义 为 : 


广义 主动 力 
0, - YF, - (»), (7. 62) 
广义 惯性 力 
QT YF D (7.63) 
3t CT. 61) 变 成 如 下 形式 E 
Y, (0, + 0,80, =0 (7. 64) 


式 (7. 64) 表示 广义 坐标 中 的 达 明 贝尔 — 拉 格 朗 日 原理 。 对 于 完整 的 系统 ,广义 坐标 相 

互 独立 , 虚 位 移 ôq, 可 以 任意 选择 , 式 (7.64) 成 立 , 则 只 能 为 
Q, *Q,; 20,5 2 1,2,-,n (7.65) 
式 (7. 65) 称 为 遍 恩 方程 (Kane's Dynamical Equations) , 是 在 广义 坐标 中 受理 想 、 完 
整 \ 双 面 约束 的 非 自 由 质 系 动力 学 方程 组 。 对 有 个 广义 坐标 的 系统 ,每 个 广义 坐标 对 应 一 
个 方程 ,而 且 相 互 独立 ,是 描述 非 自 由 质 系 动力 学 最 少量 的 方程 。 在 常用 坐标 系 中 ,如 笛 卡 
儿 坐 标 系 中 也 能 建立 非 自 由 质 动力 学 方程 组 ,但 各 方程 往往 互相 不 独立 ,必需 满足 约束 条 件 
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的 限制 ,所 建 的 方程 数 大 于 。 式 (7. 64) 中 第 一 项 D, Qa 的 物理 意义 是 广义 主动 力作 用 
于 质点 的 广义 功 ,质点 运动 ,因而 立 Qa ,89, 与 动能 有 关 。 考 虑 广义 惯性 力 为 
Qu, = YR. e (V;); = > - m;ā; * (vj), = > MT, E. 


d r; i 
-ql Xm. Poi) e Bm aat (7. 66) 
将 式 (7.58) 代入 式 (7. 66) ,其 第 一 项 能 用 动能 表示 为 


m 


E PLE ar PLE ar P PELLE 


ðq; 
Gui XE v, (7. 67) 
X7. 66) 第 二 项 , 乔 能 交换 对 时 间 和 广义 坐标 qu 求 导 顺序 , 即 得 出 
drory | or: 
alos) x 0q, UR 


第 二 项 可 以 用 动能 表示 为 
S or.d(ór| xv :nn 39/1 s: 
2m, T; ° Ma 5 2 m, F; àq, id 2; A ° rj = 211 Yi ym v, * v.) CT. 69) 
在 矢量 水 数 r; 中 ,广义 坐标 与 时 间 之 间 并 不 独立 ,坐标 随时 间 而 变化 。 能 否 交 换 求 导 顺 
序 , 拉 格 明 日 给 出 了 严格 的 证 明 , 式 (7.55) 对 广义 坐标 q, 求 偏 导 。 





ðr, E 4X. ry F; 

d, 

0q, Al 09,0q, * ataq, (7. 70) 
aqar) = s 3 (35); + $(3)- : EL oT: 

dt 3g, f=1 9q, V ðq, ðt \ ðq, Ezi 09,0q, ðtðq, 


故 证 明了 式 (7.68) 成 立 。 
在 惯性 坐标 系 下 ,系统 的 动能 定义 为 


I 3 Fm “Vv 
因此 ,将 式 (7.67) 和 (7.69) 代入 式 (7. 66) ,得 


We o RT 
Qui dt ðq, ðq, (7. 71) 
将 式 (7.71) 代入 式 (7. 65) ,得 到 第 二 类 拉 格 朗 日 方程 为 
二 aT m na 
dt ðq, ðq, E Q, , k E 1, TL (7, 72) 
展开 写 为 
d oT 07 -- or; 
LM ANE E aa T. 73 
dt oq, ðq 21 0q, a) 
d ariar: AF RER ETR (7. 73b) 


dt ðq 0q, A ^ 9q, 
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第 二 类 拉 格 朗 日 方程 是 在 广义 坐标 中 用 动能 和 广义 力 表示 的 质 系 动力 学 方程 ,等 效 变 
换 式 (7.58) 和 (7. 68) 是 其 推导 过 程 的 关键 之 处 ,所 以 称 为 拉 格 明日 方程 。 

【 例 7.7】 如 图 7. 14 所 示 ,平面 2R 机 械 臂 , 杆 长 度 a, =a =1 m, ROS m, 2m; -1 kg, 
重力 加 速度 为 29.81 m/s 。 为 了 描述 系统 的 瞬时 位 形 , 定 义 两 个 广义 坐标 qi ,9q ,运用 第 
二 类 拉 格 天 日 法 推导 动力 学 方程 。 














1. 运动 学 分 析 

参考 例 7.4, 得 知 杆 1、 杆 2 的 杆 角速度 和 质心 点 线 速度 为 

0 —$, 

e, -| 0 ê Sod € 

qi 0 
0 -0415$,01706,5121 
wa =| 0 P aicl191+accliag12 

d12 0 








式 中 i = 杆 1 的 动能 T, 表示 为 


1 Nc» R | l 1 
T, gi ei mi 1&7 €T, e, = (Fm tzai) qi 


FF 2 的 动能 T, 表示 为 


l . AM 1 ; c3 1 
T, — Uus : C+ €»; c; =F m (aigi *ac,d1 *24,46,0,41d1, ) *5 m,a2d1; 


系统 的 总 动能 为 
T=T,+T, 
根据 式 (7. 73) , 左 侧 可 以 按照 步骤 求 导 ,=1,2。 
2. 广义 主动 力 
在 惯性 系 下 ,作用 于 杆 2 的 主动 力 包 括 重力 G, =m8g, ,输入 力矩 71,=72ei ,作用 于 杆 1 
主动 力 包括 重力 G =mig, MADE nre ,还 有 杆 2 对 杆 1 的 反作用 力矩 -7 , 则 与 9 对 
应 的 广义 力 为 











2 
0€), 0C, 0€0, dW oc, 
一 R a (H = . 一 + G, *: — - .—— 十 . 一 4-G., : — 
Q; 2, ie i)i ET àq, 1 ðq, 7) ðq, T2 ðq, 2 àq, 
5 q 对 应 的 广义 力 为 
2 
0€0, 0C, AF dW, 0C; 
= 
e: 2. MIO 0q; 0q» lia 0q» "s 0q» á 0q; 





以 上 步骤 经 过 整理 ,可 以 得 到 式 (7. 22) 。 
【 例 7.8】 图 7. 14 所 示 平 面 2R 机 械 辟 ,运用 凯 恩 方程 推导 动力 学 方程 。 
根据 公式 (7. 63 ) ,广义 惯性 力 定义 为 
Qi = YF. (9), 
对 于 2R 平面 机 械 臂 为 
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ac Jw 
9,47 Y Que yt A ,k=1,2 
zl ðq; 0q, 
式 中 ,惯性 力 表示 ;i ;为 
F;;7-mj;c 
惯性 力矩 表示 为 
ET -I,«, -«, Xl, 
对 于 平面 2R 机 械 臂 ,上 述 各 变量 推导 见 例 7.4。 则 
àc, 0c, óc, am， 
in =F, 6 a th a TH ur m 
Qo; " 0q, B 09) 091 ðq; 
ac ð ac ð 
Qin =F ni m à ia : — Hl. : TA 
i ”  0q, l ðq "  0qQ, 0q; 
根据 凯 恩 方 程 ,得 
Q; tQ... -0 
Ot» =0 


式 中 ‚Qı ,Q; 见 例 5 fe 以 上 公式 经 过 整理 ,也 可 得 到 公式 (7. 22) o 
7.3.3 Wkhr-hrk& BH HO; e 


如 果 以 广义 坐标 4=[q，9% … 4 ] 表示 质点 系 的 位 置 , 则 质点 系 的 势能 可 以 写 为 
广义 坐标 的 函数 , 即 
= 
如 采 公 式 (7.72) 右 侧 的 某 些 力 来 自 有 势力 ,Q, 可 以 拆 分 为 两 部 分 0 和 0@. ,前 者 由 U 
引起 ,为 系统 保守 广义 力 , 后 者 为 与 广义 坐标 对 应 的 非 保 守 广 义 力 , 并 且 





.90 
QO, ðq, 
则 式 (7.72) 可 以 写 为 
ear Eg (7.74) 


dt ði 9q, 0q, i 
因为 9 =0, 引 入 拉 格 朗 晶 函数 也 为 





L-T-U Ep 
公式 (7.74) 可 以 写 为 
doL oL. 
dtóq, ðq, “由 VU 


AC. 76) 称 为 欧 拉 - 拉 格 朗 日 方程 。 对 于 常规 的 工业 机 器 人 ,其 驱动 关节 变量 可 以 作 
为 系统 的 三 义 坐 标 , 作 用 于 驱动 关节 的 力 或 力矩 即 为 广义 驱动 力 , 式 (7. 76) 可 以 写 


二 (7. 77) 
式 中 ,7 表示 作用 于 第 左 个 关节 的 力 或 力矩 。 


【 例 7.9】 图 7. 14 所 示 平 面 2R 机 械 臂 ,其 动能 的 表示 见 例 7. 7 ,势能 为 
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U = > mg * 4 
TF 1 的 势能 U, 为 
U, 2m,g * c, 2m;ga,,s, 
FF 2 的 势能 U, 为 
U,-m;g :c,-2m;ga,s,*m;ga,,s; 

2R 机 械 臂 总 的 势能 为 

U=U, +U, 
其 拉 格 朗 日 函数 为 

L-T-U 
利用 公式 (7.77) 可 以 推导 出 ZR 机 械 辟 的 动力 学 方程 ,其 广义 驱动 力矩 定义 为 

O.,=7=|7, Gl 


对 于 同一 个 机 械 臂 采用 不 同 的 动力 学 方程 ,所 得 出 的 计算 结果 一 致 ,因此 应 灵活 选择 并 
运用 合适 的 动力 学 建 模 方法 。 

7.3.4 n 自由 度 机 械 臂 动力 学 方程 

基于 欧 拉 - 拉 格 天 日 方法 推导 nn 自由 度 串 联机 械 臂 的 动力 学 方程 。 空 间 自由 度 机 器 
人 ,假设 第 i 杆 质 心 在 坐标 系 |1| 的 位 置 拓 量 为 r, ,其 速度 和 角速度 为 mw, 和 om, ,系统 广义 坐 
标 为 关节 变量 gqg=[g， qo o q,] ,如 图 7. 15 所 示 。 





图 7.15 nn 上 自由 度 机 械 辟 速度 的 表示 
当 建 立 系统 D-H 坐标 系 后 ,可 利用 第 4 章 的 知识 得 出 杆 i 质 心 点 速度 与 广义 速率 之 间 
的 映射 关系 为 


=Ju(g)9 (7.78a) 
w;=J,(q)q (7.78b) 
则 杆 i 的 动能 为 
T; => M Va Vito; l «0, ) (7.79) 
将 式 (7.78) 代 人 式 (7.79) ,得 
T,-7 d UJ (a) Jig) (4) 13, (4) d (7. 80) 


式 47.80) 中 ,所 有 和 天 量 ,包括 惯性 张 量 ,在 参考 坐标 系 111 下 表示 。 所 有 杆 的 动能 可 
以 表示 为 


n 1 R n " 
T = 2. T; 731 2. LTR (gq) J.(q) Pia CIKPFRTI q (7.81) 
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杆 i 的 势能 可 以 表示 为 
U; = m;g Tr, 
所 有 杆 的 势能 表示 为 
U- YU 
XX CT. 81) 简写 为 
T-74'M(a)d 


(7. 82) 


(7. 83) 


式 中 ,M(g) = > [mJ (q) J.(q) Fda (4) LJ) ] 为 对 称 正定 矩阵 ,依赖 于 广义 坐标 


q , 则 拉 格 天 日 函数 为 
1 
L= ia T (4)q4;4; - U(q) 
式 中 
ta ; 
-54 M(4)4 = 了 LS m, (q)q:q; = 
ij 
mi, ^* my ** m; qi 
I ui i : 
5314 di CU qn | m, *"* my cC Mia qk: 
mi € M nk P M an q, 
m4141q; Tue m3 ,q; *ste + m4, 

1 本 E "^ 

o| T Mulige tT t MKII t 77 tm49,4, 

t Mindin T Mindin t: ~ m, d, 

———Ó 得 
X i - à mug, 
对 时 间 微 分 ,得 

d ðL " d... P ðm, 
de aqi T 2I Dd = bam + D ag 


LI k J- XIRRI, 19 





idj 
0g, 2 ^7 ðq?” ðq; 


基于 欧 拉 - 拉 格 天 日 方程 ,系统 动力 学 可 以 写 为 


通过 交换 县 加 的 次 


om l om; U 
YE 3 — 2 8q, dp às, 7 


FF ,并 利用 和 矩阵 M 对 称 的 特性 ,得 











(7. 84) 


(7. 85) 


(7. 86) 


(7. 87) 


(7. 88) 
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Y earr d; 15 fon onu qug, 


























ðq; ðq; 
因此 
om, 1 | 3 195 zm ðM; - IT 
EE. q;q; = 十 a qiq; [ 4. 89) 
之 | ðq; 2 öğ; ; 2, 2 ðq; ðq; 0q, | 
一 类 克里斯托弗 (Christoffel Symbols) 定义 为 
iu ss 2m 80. Mud (7. 90) 
ôq; ðq; 0q, 
定义 Gi 为 
b id LA. 9] ) 
dk 
系统 的 欧 拉 - 拉 格 明日 方程 可 以 写 为 
Y mq, * È "34,4; + Gi =7ik = 1 n (7. 92.) 
J UJ 


简写 为 矩阵 形式 为 
M(q)q + V(q,3) +G=7 
[9617.10] 图 7.14 所 示 为 平面 2R 械 臂 ,运用 以 上 推导 过 程 建立 其 动力 学 方程 。 
首先 分 析 系 统 Ju JU = 1,2) ,得 








0 0 — Me 0 
Ja = [ ] ,J = acc, 0 
1 O0 0 0 
0 0 Z QIS] —06,85 T Qe,512 
Ja = [ ] Jo 7| ac, + ac, Ci) Qc, C15 (7. 93) 
Mo 0 0 
平面 2R 机 械 臂 总 动能 为 


2 n 
T- YT -54' Y, Um, (4) 4,(4) * Ju (4) 1,J,(4)] à 
rz GELS 


XP.q-[q «1 ,所 有 的 量 表示 在 惯性 参考 坐标 系 下 , 杆 件 的 惯性 张 量 去 ,因为 2R 机 械 臂 
为 平面 机 构 ides GD LUE p) 的 计算 可 以 简化 为 (J; ) ,J (q) J,(q) , (L3) a = 大 为 杆 件 的 
REKE L 第 三 行 第 三 列 项 ,进一步 得 到 和 矩阵 M 为 


M = mida da - uda * Lua 十 (7. 94) 
代入 式 (7.93 ) 计算 可 以 到 式 (7. 22) 中 的 M 和 矩阵 。 下 面 推导 克里斯托弗 的 符号 。 
E ðM, 
Vin ? ðq, =0 
E om; 





var Uu T. ðq =—m2aiac,s, =h 
2 


四 
0q 2 ðq, 
网 
T 9g! 2 ðq 


130 工业 机 器 人 技术 


] om; 
"m 792275 3g A 
1 


| 10m; 
Uan = 2 ðq, = 
杆 1 和 杆 2 总 势能 见 例题 7. 8 ,根据 公式 (7.91) ,得 


G,-m, Qc 8C, *m;g( a,c, TG. Cl12 ) 


0 





0 (7. 95) 





(7. 96) 


G; —-m580(,C1, 
HX C7. 94) ~ (7.96) ,可 以 得 到 2R 机 械 臂 动力 学 方程 为 
muqit+mi2q92+V01219192+02119291+022192 十 Ci =T; 
mn2191 十 mi229g2 0,54, *G; — T; 

[517.11] 图 7. 16 所 示 为 平面 2R 机 械 臂 ,与 图 7. 14 所 示 的 平面 2R 机 械 臂 驱动 方式 
不 同 ,其 第 2 关节 的 驱动 电机 安 转 在 基 座 上 ,通过 同步 带 或 者 其 他 方式 对 第 2 关节 远程 驱 
动 ,其 坐标 表示 方式 也 与 图 7. 14 不 同 , 试 建立 平面 2R 机 械 臂 远程 驱动 动力 学 模型 。 

自 先 分 析 系 统 J ,J,(i=1,2) ,得 

















0 0 -acs! 0 
J42|90 | „Ja =| aac 0 
1 0 0 0 
0 0 -Qls! -Qc,s2 
J,,7|0 1 Mua ib Qc, C5 | 
g 1 0 0 
依据 式 (7. 83) ,计算 得 到 的 惯性 张 量 和 矩阵 为 
m, ac. 4*m4a; +I, maiac, cos( q;-q, ) 
B Wess dn ) maç, +l, | 
依据 式 (7. 90) ,一 类 克里斯托弗 符号 计算 为 
Va 
Vn = Wa = 
2221 三 一 ma2G1Qc Sin( q,—q1) =h 
vi =—h 
9,54 -0 
055 c) 


坐标 q,,q. 所 表示 的 杆 1、 杆 2 的 总 势能 为 
U-m,;ga,,s,*m;g(a,s, as; ) 
根据 式 (7. 91) ,得 到 G,,G, 为 
Ci = 人 miac, +m2a1 ) gc, 
G, 一 128Qc，C2 
远程 驱动 平面 2R 机 械 臂 的 动力 学 方程 为 


LE s.. . 2 
mq, 十 721292 十 022192 +G =T] 
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" x ;d 
mq *m5q,tviq, +G, =T 





图 7. 16 远程 控制 2R 平面 机 械 臂 


本 章 小 结 


本 章 从 串联 机 械 臂 静 力 学 人 手 ,分 析 了 机 械 臂 单 杆 件 所 受 的 力 及 其 表示 方法 ,进而 研究 
了 和 牛顿- 欧 拉 方 程 在 机 械 臂 动力 学 建 模 中 的 应 用 。 进 一 步 ,研究 了 一 类 、 二 类 拉 格 朗 日 方程 
及 欧 拉 - 拉 格 朗 日 方程 在 串联 机 械 臂 动力 学 建 模 中 的 应 用 ,为 复杂 多 刚体 机 械 臂 的 动力 学 
建 模 葛 定 了 基础 。 


第 8 章 MAET 


机 器 人 控制 器 的 设计 通常 分 为 运动 控制 和 动态 控制 两 类 。 运 动 控制 不 考虑 机 右 人 动力 
学 特性 ,按照 期 望 轨迹 和 实际 轨迹 的 偏差 进行 负 反 馈 控制 。 其 优点 是 控制 方法 简单 ,易于 实 
现 。 但 有 两 个 明显 的 缺点 :中 难于 保证 所 控制 的 机 器 人 动态 和 静态 性 能 良好 ;@) 需 要 较 大 的 
控制 能 量 。 动 态 控制 依赖 于 动力 学 模型 ,是 基于 控制 对 象 模 型 的 非 线性 控制 方法 ,克服 了 运 
动 控制 的 缺点 ,但 在 控制 过 程 中 需要 实时 进行 机 器 人 动力 学 模型 菜 些 项 的 计算 ,计算 量 大 ， 
实时 控制 较 困 难 。 


8.1 二 阶 线性 系统 


在 考虑 机 器 人 控制 问题 之 前 , 先 考 虑 一 个 简单 的 机 械 系 统 。 图 S. 1 所 示 弹 簧 -阻尼 - 质 
量 系统 ,质量 为 mm 的 质量 块 通过 刚度 系数 为 丰 的 弹 自 连接 于 基 座 ,与 基 座 表面 的 摩擦 因数 
为 ,假定 摩擦 力 正 比 于 质量 块 的 速度 ,质量 块 的 零 位 置 和 位 移 如 图 8. 1 所 示 , 则 此 对 象 的 
动力 学 方程 为 


图 8.1 弹簧 -阻尼 -质量 系统 
mx+bx+kx=0 EE. 13 
式 (8. 1) 为 二 阶 线性 常 系数 微分 方程 ,此 单 自由 度 的 开 环 动力 学 系统 通过 二 阶 线性 微 
分 方程 描述 ,使 用 此 人 简单 的 机 械 系 统 , 目 的 是 回顾 基本 的 控制 概念 ,此 系统 受 弹 簧 刚度 系数 
k ,阻尼 系数 上 以 及 质量 块 的 质量 m 大 小 的 影响 ,如 果 系 统 受 到 外 部 的 扰动 ,刚度 系数 小 ,而 
阻尼 系数 大 ,质量 块 将 以 缓慢 的 方式 返回 静止 的 位 置 ; 反 之 ,刚度 系数 过 大 ,摩擦 阻尼 小 ,在 
回 到 平衡 位 置 之 前 ,质量 块 可 能 要 来 回 振荡 几 次 。 在 系统 中 ,微分 方程 的 解 x(i) 确 定 质 量 

块 的 位 置 。 为 了 研究 如 式 (8. 1) 所 示 的 微分 方程 ,在 复 变 函数 中 ,通过 拉 氏 变换 得 
ms +bs+k=0 (8.2) 

方程 (8.2) 的 两 个 根 为 
: b+Vb: -Amk : b-b -Amk 
Ju 2m Fw 2m 


x 的 解 可 以 表达 为 *=cie" +cze” ,其 中 参数 c ,c 值 取决 于 质量 块 的 初始 位 置 和 速度 。 如 果 


(8.3) 
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51,52 为 不 相等 的 实 根 , 即 b^ »4mk ,系统 为 过 阻尼 系统 ,摩擦 占 主导 地 位 ,系统 行为 特性 趋 于 
稳定 。 如 果 b?=4m%k, 此 时 方程 具有 相等 的 实 根 ,刚度 和 摩擦 为 “平衡 ”, 系统 具有 最 快 响应 
的 非 振 荡 特 性 , 称 为 临界 阻尼 系统 。 如 果 s oss 为 复 根 , 即 /<4m, 弹簧 刚度 系数 占 主 导 地 
位 ,系统 行为 特性 表现 为 振荡 。 式 (8. 1) 可 以 写 为 规范 的 方程 形式 为 





X+2lw x+w.x=0 (8.4) 
RP= eo, 和 分 别称 为 系统 固有 频率 和 阻尼 比 。 当 =1 时 ,系统 为 临 


界 阻尼 ; 当 Z>1 时 ,为 过 阻尼 系统 ; 当 L<1 时 ,时 为 欠 阻 尼 系 统 。 


8.2 二 阶 线性 系统 的 控制 


再 次 回 到 图 8. 1 所 示 的 系统 , 当 系 统 模型 中 mb k 三 个 参数 不 能 保证 s, s; 为 实 根 时 ， 
系统 为 从 阻尼 或 振荡 系统 ,此 时 如 果 外 部 存在 扰动 ,系统 为 振荡 系统 ;假定 考虑 通过 控制 执 
行 器 在 弹簧- 阻尼 -质量 块 上 作用 一 个 力 ./, 当 外 部 存在 扰动 时 ,实时 改变 力 j 的 大 小 和 方 癌 ， 
最 终 可 以 使 系统 保持 在 初始 静止 位 置 ,此 时 系统 的 动力 学 方程 变 为 


图 8.2” 受 作用 力 的 弹簧 -阻尼 -质量 系统 
mx-bx-ekx =f (8.5) 
下 面 考 虑 一 个 问题 ,如 何 加 载 作 用 力 f 于 质量 块 上 ,能 使 系统 成 为 一 个 稳定 系统 ,假定 
可 以 利用 传感器 观测 出 质量 块 的 实时 位 置 .速度 的 输出 x,x, 并 期 望 执行 器 作用 于 质量 块 的 
力 f 为 传感器 观测 的 x,x 的 函数 ,表示 为 
f- -k,x-k,x (8.6) 
XX HP Lk, k, 分 别 为 位 置 比例 增益 与 速度 比例 增益 ,为 可 以 调节 参数 。 整 个 系统 的 控制 框图 
如 图 8.3 所 示 ,作用 于 质量 块 的 力 的 大 小 取决 于 质量 块 的 位 置 和 速度 。 


X 





图 8.3 PD 反馈 控制 系统 


当 控制 系统 通过 执行 器 作用 于 质量 块 的 力 符合 式 (8.6) 时 ,将 式 (8.6) 代 入 式 (8.5) ,得 
mx-bx--kx 二 —k, x—k,x (8.7) 


式 (8.7) 进 一 步 写 为 
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mx- (bk, ) x ( kk, ) x z0 (8.8) 
简化 为 
mx-b'x-k'x -0 (8.9) 
式 (8.9) 表 示 系 统 的 闭环 微分 方程 ,6 =b+k,,k'=k+k, 分 别 为 闭环 的 阻尼 和 刚度 系数 ， 
显然 ,调整 参数 k, 可 以 使 式 (8.9) 所 表示 的 系统 的 根 为 实 根 ,满足 系统 的 稳定 条 件 。 当 
如 图 8.2 所 示 的 系统 为 开 环 系统 时 ,在 拉 氏 域 ,输入 f 和 输出 x 的 传递 水 数 为 
x(s). Bl ae 
fCs) ms’ +bs+k 
式 中 ,x(s) ,f(s) 是 时 变量 位 移 x 和 力 f 的 拉 氏 变换 。 当 系统 为 闭环 控制 , 且 存 在 外 部 扰 时 ， 
系统 控制 原理 图 如 图 8.4 所 示 。 





(8. 10) 





图 8.4 受 扰动 的 PD 反馈 控制 系统 
图 中 ,f(s) 表 示 作 用 于 系统 的 扰动 力 。 扰 动力 f(s) 与 闭环 系统 输出 x(s) 的 传递 函数 为 
x(s) -G(s) 
E A MEN (8. 11) 





fs) 1+(k,+k,s)G(s) 

[58.1] 如 图 8.2 所 示 , 如 果 系 统 参 数 为 m=1,b=1,k=1, 假 设 系统 为 团 环 控制 , 闭 
环 系统 的 刚度 系数 给 定 为 k'=16, 并 且 为 临界 阻尼 系统 =1, 试 求 此 时 控制 系统 内 部 增益 参 
数 ,,k, 的 值 ,并 编写 程序 仿真 此 系统 开 环 和 闭环 时 , 当 外 部 存在 扰动 时 的 啊 应 情况 。 系 统 
为 临界 阻尼 系统 , 则 

b'=2,/mk' =8.0 
b'=b+k,=8.0 

因此 ,k,=15,%k,=7。 如 果 系 统 为 开 环 ,传递 函数 为 式 (8. 10), 当 质量 块 受到 冲击 扰动 

f(s)= 1 力 时 ,此 时 f(s) 为 输入 ,输出 x(s) 变 化 通过 Matlab 仿真 , 见 表 8. 1。 
表 8.1 单位 脉冲 扰动 编程 仿真 


num 三 一 
den 5 [111] 


impulse( num , den ) 


单位 脉冲 仿真 图 如 图 8. 5(a) 所 示 ,系统 振荡 。 如 果 系 统 为 财 环 控制 ,参考 式 (8. 11) , 传 
35 PR 
x(s) " -1 
fals) s +8s+16 
当 质 量 块 受 到 冲击 扰动 f(s)= 1 时 ,输出 x(s) 变 化 通过 Matlab 仿真 ,从 图 8.5(b) 可 以 
看 出 ,x(s) 的 输出 趋向 稳定 。 
如 果 期 望 质量 块 移动 到 期 望 的 位 置 x ,采用 PD 反馈 控制 ,系统 的 输入 为 e=xs-x, 控 制 
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0.1 0 
0 
-0.021 
-0.1| 
-02 -0.04 
$ -0.3 R -0.06 
-0.4 
-0.08 
-03 
-0. 70.1 
" 00 4 6 8 10 12 0.5 ] 1.3 2 2.9 
时 间 /s 时 间 /s 


图 8.5 单位 脉冲 扰动 仿真 图 
系统 框图 如 图 8. 6 所 示 





图 8.6 PD 位 置 控制 框图 


当 . 广 (s)=0, 系 统 财 环 的 输出 *(s) 与 期 望 输入 xs(s) 的 传递 函数 为 
x) . COO 
x,((s) ] - ( k, -k,s) G( s) 
如 果 式 中 ,=15 ,k,=7, 则 传递 函数 可 以 表示 为 
x(s)  7s+15 
x,((s) s+85+16 
对 于 阶 跃 输入 信号 xe(s)= 1《s ,输出 x(s) 的 响应 情况 ,Matlab 仿真 编程 见 表 8. 2。 
表 8.2 单位 阶 跃 信号 输入 编程 仿真 











num 三 [7 15] 
den = [18 16] 


step( num, den) 


仿真 图 如 图 8. 7 所 示 , ZR SEO e XE E RA , T. 24 08 2 PR Afi) 2 BT EK i xs=1 时 ， 
系统 输出 并 不 为 1 ,这 是 因为 PD 控制 方法 无 法 消除 系统 稳 态 误差 造成 的 。 
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0 l 2 3 
时 间 /s 


图 8.7 ŽANRE SAARE 


8.3 ”机 器 人 独立 关节 控制 


如 果 机 械 臂 采用 开 环 的 控制 方式 ,为 了 实现 期 望 的 轨迹 ,作用 于 关节 的 力 可 以 通过 动力 
学 模型 直接 计算 得 到 ,动力 学 方程 表示 为 
M q,*V(q, P, +G(q4)=T 
RP gardada 为 所 规划 的 期 望 关节 位 置 、. 速 度 与 加 速度 ,作为 控制 系统 的 输入 变量 。 假 如 
使 用 上 述 公 式 计 算 作用 于 关节 的 力 ,在 动力 学 模型 中 所 包含 的 物理 量 ,如 与 机 械 臂 杆 件 的 长 
度 、 质量 和 转动 惯量 以 及 关节 中 的 摩擦 力 , 还 有 减速 器 回 差 及 外 部 扰动 等 物理 量 的 不 确定 
性 ,在 此 力 的 作用 下 ,机 械 臂 可 能 实现 不 了 期 望 的 轨迹 。 因 此 ,机 械 臂 的 控制 采用 闭环 反馈 
的 控制 方式 是 必要 的 ,控制 系统 的 输入 为 期 望 位 置 速度 与 实际 位 置 4、 速度 4 的 偏差 ,期 户 
的 位 置 .速度 为 规划 得 到 ,实际 位 置 .速度 通过 传感器 测量 得 到 ,偏差 表示 为 
€-Q,-q,€-Q,-d 

本 节 研 究 简单 的 .可靠 的 机 器 人 独立 关节 控制 方法 ,在 此 控制 方法 中 ,对 机 械 辟 每 个 关 
节 的 控制 都 是 单 输入 单 输出 系统 ,运动 中 任何 其 他 杆 件 对 此 关节 的 耦合 影响 看 成 外 部 的 扰 
动 ,此 控制 方法 适应 于 非 快 速 运动 的 应 用 领域 ,其 控制 原理 如 图 8. 8 所 示 。 


外 部 的 扰动 ,As) 





图 8.8 机 器 人 独立 关节 控制 原理 图 


8.3.1 水 磁 下 流 电 机 模型 


如 果 每 个 关节 驱动 采用 永 磁 直流 电机 驱动 , 永 磁 直流 电机 是 目 动 控制 中 帝 用 的 执行 原 
件 ,输入 的 电压 信号 转换 成 转轴 的 角 位 移 或 者 角度 输出 ,一般 要 求 直 流 电机 具有 优良 的 快速 
啊 应 特性 , 即 要 求 电 机 的 机 电 时 间 稼 数 小 ,转动 抢 转 动 惯量 比 大 , 调 速 范围 宽 。 除 此 之 外 , 电 
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机 转 和 矩 脉动 小 ,低速 运行 平稳 。 永 磁 直 流 电机 电气 原理 如 图 8.9 所 示 。 系 统 动态 方程 如 下 : 
V= V, Ri, «LS (8.12) 


do, 


s (8.13) 


V, sk, -k 
(8. 14) 


(8. 15) 











水 磁 直 流 电 机 动态 过 程 中 的 电 枢 电压 (V) 电感 (H) 电阻 (0) R 
应 电动 势 (V) 以 及 电 枢 电流 (A); 

wm 一 一 电机 机 械 角 速度 ; 

9, 一 一 电机 机 械 角 度 , rad ; 

Tn 一 一 电磁 转 矩 的 瞬时 值 ,N * m; 

7 一 一 负载 转 矩 ,包括 电机 轴 上 外 部 输入 转 矩 和 恒定 阻力 转 矩 ,Nm; 

B, 一 一 阻力 系数 ,正比 于 转子 机 械 角 速度 ; 

J 一 一 转子 转动 惯量 kg : m^; 


hk 一 转 和 矩 系数 ,2 全，( Nm/amp); 
ma 





式 中 V,L,R,V, ,i, 














k, ——] RS; Fb, zl zo P 


' 60a ^ 
其 中 BB 一 一 对 应 于 定子 的 磁 通 ; 
Pp 一 一 极 对 数 ; 
NV 一 一 电 枢 绕组 总 导体 数 ; 
a 一 一 电 枢 绕组 的 并 联 支 路 对 数 。 








图 8.9 永 磁 直 流 电机 电气 原理 图 
对 已 制 成 的 永 磁 直 流 电 机 来 说 ,在 电机 正常 运行 中 , 当 不 考虑 饱和 时 ,@ 不 变 ,h, Le, 为 
常数 。 综 合式 (8. 12) ~ (8. 15 ) ,直流 电机 输入 电压 V, 转 子 机 械 角速度 w。 和 电磁 转 矩 m, 


的 关系 为 
+T 十 一 Cu (8. 16) 


AHP c, 2 L/R 定义 为 电气 时 间 常 数 ,由 于 电 枢 绕组 电感 工 很 小 ,电气 时 间 常 数 很 小 ,所 以 常 
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常 略 去 电机 的 电气 过 渡 过 程 ,近似 认 为 ZAR=0, 此 时 电压 、 角 速度 与 电磁 转 矩 的 关系 为 
ko kk, 


qm wW 


(8.17) 
R EH. 
根据 式 (8. 17) ,得 出 典型 的 直流 电机 转 矩 -角速度 曲线 如 图 8. 10 所 示 。 在 一 定 温 度 
下 ,普通 永 磁 直 流 电机 的 磁 通 基本 不 随 负载 而 变化 ,与 并 励 直 流 电机 相同 , e, , 心 基本 保持 
不 变 , 在 给 定 的 输入 电压 了 下 ,输出 的 电磁 力矩 随 转速 的 增加 而 下 降 。 





maxr,, 


max w 
i max Wy, m Um 


图 8. 10 ” 转 矩 角速度 曲线 
式 (8. 12) ~ (8. I5) 进行 拉 氏 变换 , 令 初始 条 件 为 零 ,整理 得 


V(s) -V,(s)2 (Ls+R)i,(s) (8. 18 ) 
V,(s)*2 k,s0, (s) (8. 19) 

T, (s) 9 k,i,(s) (8.20) 

T (s) -r(s)2 (J,s! *B,s)0, (s) (8.21) 


系统 的 外 部 输入 量 为 电压 VC s) 和 负载 转 矩 0,68) ,以 角度 0n 为 输出 量 , 式 (8. 18) ~ 
(8.21) 所 表示 的 系统 可 的 框图 的 形式 表示 为 图 8. 11。 由 图 8. 11 可 见 , 永 磁 直流 电机 本 喘 
是 一 个 财 环 系统 ,感应 电动 势 引入 与 电机 角速度 成 正比 的 负 反 馈 信 号 ,增加 了 系统 的 有 效 阻 
Ba 





图 8.11 永 磁 直流 电机 系统 框图 


8.3.2. 含 轮 系 的 直流 电机 控制 模型 


直流 电机 通过 减速 系统 连接 到 机 械 臂 的 关节 ,各 关节 减速 比较 大 ,一般 为 20 ~ 200 甚至 
更 大 ,大 的 减速 比 降低 耦 合 到 此 关节 的 外 部 扰动 力 ,并 提高 了 电机 输出 力矩 ,下 面 将 建立 含 
轮 系 的 独立 关节 控制 模型 。 图 8. 12 为 典型 的 单 关节 传动 方案 ,包括 直流 电机 ,通过 齿轮 减 
速 系统 连接 到 机 械 臂 的 关节 轴 ,减速 比 用 ~ 表示 。 电 机 输出 的 角 位 移 、 角 速度 为 gu,wn， 经 
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过 齿轮 系统 后 传递 到 关节 轴 的 角 位 移 、 角 速度 分 别 为 9,,w, ,显然 gu/6.=r, 作 用 关节 上 的 主 
动力 矩 7, ,对 电机 而 言 为 外 部 负载 ,作为 外 部 负载 传递 到 电机 转子 上 为 7,=7,Ar。J 分 别 为 
电机 的 转子 转动 惯量 ,J 为 齿轮 1、 齿轮 2 等 效 的 转动 惯量 ,两 者 之 和 为 J,=J,+J,。 





图 8.12 单 关 节 含 电机 轮 系 的 集中 模型 
如 图 8. 12 所 示 系 统 ,其 动力 学 方程 为 
J, 9, 4B, Ó, =T- =k LL (8.22) 


式 (8. 22) JIA (8. 21) 比较 可 知 ,通过 直流 电机 驱动 关 市 的 运动 ,外 部 负载 7,=7,/r AE 
化 ,电机 系统 的 转动 惯量 因为 轮 系 的 原因 有 变化 ,其 他 没有 变化 。 因 此 控制 原理 图 与 图 8. 11 
类 似 ,只 是 里 面 有 些 变 量 发 生 了 变化 。 





图 8.13 含 轮 系 单 关 放 控制 框图 





从 V(s) 到 9,(s) 的 传递 函数 为 
- o ÉGAL oS 8. 23 
V(s)  s[(Ls+R) (J„s+B„)+k,k,] UL 2980 
从 关节 驱动 力矩 7, 到 0, (s) BEES RROA 
0,5) |. -(Ls-R)/r (8. 23b) 





T.(s) s[ (Ls+R)(J,s+B, ) *k,k, ] 
电气 常数 c 非常 小 ,因此 ,对 于 多 数 机 电 耦 合 的 系统 , 令 c.=0 是 一 个 合理 的 简化 。 而 
机 械 和 常数 c, =J,/B,, 相对 于 电气 常数 较 大 。 式 (8. 23a) 和 式 (8. 23b) 进一步 简化 为 





OAR k I/R d ia 
V(s) s(J,s*B, +k,k,/R) (8. 24a) 
0,5) | -l/r (8. 24b) 


r(s) s(J„s+B„+kyk„/R) 
在 时 域 , 式 (8. 24a) (8. 24b) ZIEH — Br (AY 27 OR 
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J 0, +( + 有 1 AR)O =(k,/R)V-T,/r (8.25) 
公式 简写 为 
JO, B, LO =Lr-dr (8. 26) 
式 中 ,J.n=J ,表示 系统 有 效 的 转动 惯量 ;Bg = B+k,k,/R, 表 示 系 统 有 效 的 阻尼 ;ug = CR, 
R)V, 表 示 控 制 输入 ;dgy=7,/r, 表 示 扰 动 输入 。 


8.3.3 单 关 人 独立 控制 模型 


基于 上 面 所 建立 含 轮 系 直流 电机 模型 ,采用 PD ,PID 反馈 控制 方法 ,研究 于 关节 机 器 人 
定点 跟踪 的 问题 。 定 点 跟踪 是 系统 跟踪 常量 或 阶 牙 指 令 9 ,用 于 控制 点 到 点 的 运动 。 如 图 
8. 12 所 示 ,假定 所 控制 的 机 械 臂 关 节 为 第 k(k=1,2,…,n) 个 关节 ,参考 第 7 章 内 容 , 机 器 人 
动力 学 方程 和 电机 的 动力 学 为 

2 m, 十 z v3,0,6,, +G, =T 


(8.27) 
J ad + (Bs + hk/ Ri) = ka R V — Talt 
式 中 M, a = T0 ko 上 述 方程 可 以 合 写 为 
1 
p F -mu(g,)] On + (B, + kk, , /] Ry) One = Er/ R,V, — dog, (8. 28) 
k 
式 中 daa 定义 为 
1 » T" 
dg, = zi > Myj + vj,0, 0, + G,) (8. 29) 
per i 


在 式 (8.28) 中 ,项 J+-5mw(0,) 是 关节 变量 0, 的 函数 , 较 大 的 减速 比 7 能 降低 此 项 对 


系统 的 影响 。 在 系统 设计 中 ,此 项 经 常 定 义 为 均值 常量 ,或 者 用 有 效 的 惯量 Jm 作 为 实际 惯 
量 的 近似 值 。 式 (8. 28) 中 各 项 进一步 等 效 为 


l 
Jn Jut amat 0, ) 
k 


Bay =B me bk ke / Ry MSS 


la =k RV, 
将 式 (8. 30) RACS. 28) ,并 省 略 下 标 , 对 于 机 械 臂 单 关 节 的 控制 模型 ,可 以 表示 为 
JÓ-BÓ -u-d (8.31) 
Thr 2H 8 hl 2 A fn] HER] E rn IER P] LS] 8. 14. 所 示 。 


d 
Ls+B s 


图 8. 14 独立 关节 控制 简化 的 原理 图 


u v 
(os 


m 


8.3.4 PD 控制 器 


使 用 PD 控制 方法 ,控制 输入 w 在 拉 氏 域 给 定 为 
u(s)- (k,*k,s) [0,(s) -0(5) ] (8. 32) 
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AF, k, k, 分 别 为 比例 和 微分 增益 ,其 闭环 控制 框图 如 图 8. 15 所 示 , 输 入 0, 与 输出 9 的 传 
递 函 数 为 
0,5) - k,s-k, k,s/ J+w’ 








Rue a (8.33) 
0,(s) Js -(B«k, )stk, s -2Lo,5*0, 
式 中 ,w=Vk,/J ,阻尼 比 定义 为 
E (8.34) 
* 73 Jf 


在 系统 中 ,固有 频率 o, BHE HE ¢ 机 k,,k, 确定 。 如 果 k, 增 大 ,系统 调整 时 间 
减少 , 啊 应 加 快 ,而 %, 增加 ,系统 阻尼 比 增加 ,抑制 了 系统 的 超 调 量 。 合 理 选择 ,kk, 的 参数 
值 ,能 保证 c, ,i 满足 系统 调整 时 间 和 超 调 量 的 要 求 。 对 于 临界 阻尼 ,此 时 k, - 2. b J -B. 
PD 控制 方法 广泛 用 于 机 器 人 控制 中 。 


d(s)= (s)| 





图 8.15 PD 独立 关节 控制 原理 框图 
从 控制 框图 可 以 看 出 , 当 rs 天 0， 0, F Ta — 其 输出 0 表示 为 





6(5)- teo s) yr G) (8.35) 
式 中 ,p(s) 表 示 系 统 的 特征 多 项 式 ,为 p(s)= Js*+(B+k hey 
e(s)= Buls) -0(s)= ts0(s Fe yr (8.36) 
对 于 阶 路 输入 和 常量 扰动 , 即 
oals) rls) (8. 37) 
系统 稳 态 误差 e, 为 
e, "lim se(s)= 7" (8.38) 


比例 增益 增 大 也 可 以 减 小 稳 态 误差 ,但 ,不 能 任意 增加 到 任何 值 ,过 高 的 比例 增益 
会 引起 系统 的 振荡 。 


8.3.5 PID 控制 器 
在 PD 控制 中 ,增加 一 个 积分 项 — 
da (^, "TI SIDE s)-8(s) ] (8.39) 


IP sk; 为 积分 增益 , 式 (8. 39) 为 众所周知 的 PID 控制 方法 ,控制 框图 如 图 8. 16 所 示 。PID 
控制 能 使 稳 态 误差 为 零 ,但 积分 增益 选择 过 大 ,控制 系统 也 会 不 稳定 。 当 rs=0 时 ,系统 
闭环 传递 函数 为 
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0(s) . k, S +k, sk; 
0, Cs ) Js +( B+k, * tk sk, 





(8. 40) 


dis z(s)|. 





图 8. 16 PID 独立 关节 控制 原理 框图 


系统 的 特征 多 项 式 为 p(s)= Js - ( B-k,) s? tk sk, ,为 三 阶 系统 ,应 用 Routh-Hurwitz 稳定 
判断 方法 ,上 述 闭 环 系统 稳定 需要 满足 以 下 条 件 : 





ka PP (8.41) 
与 式 (8. 35) 类似, 系统 的 输入 -输出 为 
ks ek sk, 
Bim cay: dte p(s jes s) (8. 42) 
跟 踊 误差 为 

eR (8. 43) 

pte) ^" h s 
e, - limse( s 3): 0 (8. 44) 


PID 控制 方法 能 控制 各 个 关节 在 外 部 扰动 力 存在 时 ,运动 到 期 望 的 位 置 。 
8.3.6 PID 控制 喜 增 益 的 选择 


PID 控制 中 重要 的 问题 是 参数 有 ,,k, ,k; 的 选择 ,k; 的 大 小 必须 满足 稳定 的 约束 条 件 , 因 
此 在 系统 设计 初始 ,可 以 先 令 点 =0, 首 先 合理 选择 比例 增益 和 微分 增益 ,满足 系统 的 瞬时 特 
性 ,如 系统 上 升 时 间 .调节 时 间 以 及 超 调 量 。 在 式 (8. 34) H, B% k, Le, 表示 为 
hk cud 
keia 
通过 式 (8. 45) 可 以 知道 ,w, 在 控制 系统 中 占据 主要 地 位 ,但 其 设 定 也 要 符合 约束 条 件 。 
尽管 机 械 臂 关节 质量 大 ,但 也 具有 一 定 的 柔性 ,因此 在 选择 o, 时 ,驱动 关节 的 关节 柔性 也 是 
一 个 考虑 的 因素 ,假设 为 关节 有 效 的 刚度 , 则 关节 一 阶 响应 频率 表示 为 w =wWA ALJ ,为 了 
如 免 共 振 ,在 工程 实践 中 ,一 般 应 符合 下 列 约束 条 件 ， 
w, Sw,/2 (8.46) 
系统 中 了 了 随 春 机 械 臂 的 位 形 发 生变 化 ,可 以 取 最 大 值 。 一 旦 控制 系统 中 o, 确定 ,机 械 
辟 控 制 中 阻尼 比 一 般 选 为 临界 阻尼 , 即 Z=1。 当 ,hk 调整 好 时 ,k; 可 以 在 符合 约束 条 件 的 
范围 内 选择 ,并 且 可 以 消除 稳 态 误差 ,但 不 合理 的 选择 可 能 使 系统 瞬 态 响应 变 差 。 三 个 参数 
对 系统 的 定性 影响 见 表 8. 3。 


(8.45) 
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表 8.3 增益 对 系统 定性 的 影响 


闭环 系统 响应 上 升 时 间 调整 时 间 超 调 量 稳 态 误差 
k, 减少 不 显著 增加 降低 
k, ^ 减少 降低 不 显著 
k, 减少 增加 增加 消除 


8.3.7 前 馈 控 制 


如 果 系 统 输入 量 或 外 部 扰动 输入 量 是 可 测量 和 可 控制 的 ,系统 可 以 采用 前 馈 控 制 方法 ， 
以 克服 时 变 的 输入 或 外 部 扰动 输入 所 引起 的 输出 量 较 大 的 波动 ,其 控制 框图 如 图 8. 17 所 示 ， 
假定 r(i) 是 任意 的 参考 输入 , G(s) 表示 受 控 对 象 传递 函数 ,有 H(s) 表 示 控 制 右 传递 函数 ， 
(4s) 表 示 前 狐 控 制 传递 函数 。 在 图 8. 17 中 ,三 个 传递 函数 G(s) ,H(s) 和 F(s) 表 示 成 如 下 
多 项 式 的 比例 函数 : 








WR ONRRO 
则 闭环 输入 输出 的 传递 函数 为 
y(s)_F(s)G(s)+H(s)G(s)_ gq(s)[e(s)b(s)+ta(s)d(s) | (8.48) 
r(s) 1 -H(s) GCs) b(s)lpCs) d(s) *q(s)c(5) ] 





图 8.17 无 扰动 前 馈 控 制 原理 框图 
如 果 选 择 前 馈 控 制 传递 函数 为 


(8.49) 
即 a(s)=p(s) ,8(s)= qs) ,而 后 式 (8. 48 ) 变 为 
y(s)_ ce(s)g(s) +tp(s)d(s) "Lam 


r(s) p(s)d(s)*q(s)c(s) 
系统 的 跟 踩 误差 为 
e(s)2 r(s) -y(s)20 
由 上 式 可 以 看 出 ,采用 前 馈 控 制 , 在 系统 无 外 部 扰动 输入 下 ,其 跟踪 误差 为 零 ,与 Hs) 
参数 无 关 。 
如 采 外 部 有 扰动 的 输入 ,控制 框图 如 图 8. 18 所 示 。 





图 8.18 有 扰动 前 馈 控 制 原理 框图 


系统 的 跟踪 误差 为 
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E A A ur 
(3er) (9) 7, 26400 — DG) (8.51) 
图 8. 16 所 示 控 制 系统 ,如果 引入 前 馈 控 制 ,其 控制 框图 如 图 8. 19 所 示 , 跟 踩 误差 为 
本 l 
sje 04) Ado y egal y, (8. 52) 
; T4 
el - aua (8. 53) 





图 8.19 PD+ 前 馈 独立 关节 控制 原理 图 


【 例 8.2】 利用 Matlab 软件 进行 独立 关节 控制 方法 仿真 。 假 定 被 控 的 单 关节 包含 直流 
电机 、 轮 系 及 机 械 臂 ,最 终 建 模 所 得 到 的 等 效 模型 为 


Sie Js? Bs 


式 中 ,等 效 的 参数 为 J=0. 006 7,B=0.10。 如 果 此 关节 输入 的 指令 信和 号 为 q =0. Ssin(2mt) , 
外 部 扰动 为 rv=1, 采 用 PID 控制 方法 设计 控制 器 ,其 中 各 增益 参数 为 k=20. 0,k,=0. 5 ,k;= 
300 ,系统 的 控制 框图 如 图 8. 20 所 示 。 利 用 Matlab 编程 仿真 , 画 出 在 此 控制 方法 控制 下 , 关 
节 期 望 位 置 q 和 实际 位 置 q 曲线 以 及 跟踪 误差 e 曲线 ,仿真 程序 见 附录 Eo 






l 


图 8.20 PID 控制 原理 图 

所 得 到 期 望 曲线 、 实 际 曲线 以 及 跟踪 误差 曲线 如 图 8. 21 所 示 。 从 图 8. 21 可 以 看 出 , 采 
用 PID 控制 ,在 外 部 扰动 存在 的 前 提 下 ,系统 可 以 较 好 地 跟踪 期 望 的 输入 ,但 其 跟踪 误差 随 
期 望 轨迹 的 变化 而 波动 。 

【 例 8.3】 如 果 采 用 PD 加 前 馈 控 制 方法 设计 控制 器 ,其 中 比例 、 微 分 增益 参数 不 变 ， 
系统 的 控制 框图 如 图 8.22 所 示 。 利 用 Matlab 编程 仿真 , 画 出 在 此 控制 方法 下 ,关节 期 望 位 
置 、 实 际 位 置 曲 线 以 及 跟踪 误差 曲线 。 所 得 到 期 望 曲 线 、 实 际 曲 线 以 及 跟踪 误差 曲线 如 图 
8.23 所 示 。 从 图 中 可 以 看 出 ,采用 前 馈 控 制 ,跟踪 误差 稳定 后 ,大 小 不 受 期 望 轨迹 变化 的 影 
Wf o 
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8.21 PID 控制 期 望 \ 实 际 位 置 曲线 及 跟踪 误差 图 


0 0.2 





图 8.23 ”PD+ 前 馈 控 制 期 望 \ 实 际 位 置 曲 线 及 跟 踊 误 差 图 


8.4 动态 控制 


8.4.1 控制 分 解法 


独立 关节 控制 属于 运动 控制 的 范畴 ,方法 只 关注 对 单个 关 市 的 控制 ,虽然 在 模型 建立 过 

程 中 涉及 了 机 械 臂 动力 学 方程 ,但 其 控制 方法 不 依赖 控制 对 象 的 动力 学 模型 ,从 现在 开始 对 

于 如 图 8. 2 所 示 的 系统 控制 稍微 做 修改 ,其 控制 方法 的 组 成 依赖 于 控制 对 象 和 伺服 系统 两 

部 分 ,其 结果 是 控制 对 象 的 动力 学 参数 m,b,k 出 现在 控制 方法 模型 部 分 中 ,但 伺服 部 分 并 

不 依赖 于 这 些 参数 ,这 种 方法 也 称 控制 分 解 方法 (Control Law Partitioning)。 在 式 (8.5) 中 ,f 
重新 表达 为 

f=af «B (8. 54) 

式 中 ,a,B 是 函数 或 常量 ,此 时 对 于 系统 而 言 ,成 为 系统 新 的 输入 ,将 式 (8. 54) 代 入 式 
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(8.5) ,得 
mx+bx+kx 2 of +B (8.55) 
比较 式 (8. 55) 左右 两 人 出 ,如 果 选 择 w,B 为 
a=m,B=bx+kx (8.56) 


可 以 看 出 a,B 依赖 于 控制 对 象 的 动力 学 模型 ,新 输入 只 是 作为 一 个 单位 质量 系统 的 
输入 ,没有 摩擦 项 ,也 没有 刚度 项 ,系统 的 微分 方程 为 
x-f (8.57) 
式 (8. 57) 为 单位 质量 系统 的 动力 学 方程 ,对 其 控制 的 方法 采用 PD 控制 方法 ,此 部 分 为 
伺服 控制 部 分 ,表示 为 
f =-k,x-k,% (8.58) 
TEX (8. 58) 代 入 式 (8. 57) ,得 
x+hk,x+hk,x=0 (8. 59) 
显然 ,对 单位 质量 系统 的 控制 ,参数 ,hk, 选择 是 简单 的 ,不 依赖 于 控制 对 象 的 参数 , 当 
伺服 系统 的 阻尼 选 为 临界 阻尼 时 , 仅 需 满足 如 下 条 件 : 
k 二 
其 控制 分 解 原理 框图 如 图 8. 24 所 示 





图 8.24 控制 分 解 原理 框图 


【 例 8.4】 如 图 8. 2 所 示 , 如 果 控 制 对 象 的 参数 为 m=1,b=1,k=1 ,假定 采用 控制 分 解 
方法 对 系统 进行 控制 ,使 其 稳定 于 静心 位 置 ,如 果 系 统 伺服 控制 比例 增益 为 ,=16, 并 且 为 
临界 阻尼 系统 7=1, 试 求 此 时 控制 系统 内 部 a,B 和 增益 参数 〖, 的 值 或 表达 式 。 

依据 式 (8. 56) ,得 

a-1l,B-xtx 
系统 为 临界 阻尼 , 则 
k=, -8 

图 8. 24 所 示 系 统 仅仅 让 质量 块 保持 在 稳定 的 状态 。 如 果 要 求 图 8. 2 所 示 的 质量 块 在 
力 的 作用 下 能 跟踪 一 条 轨迹 线 ,此 轨迹 用 时 间 郴 数 x,(1) 表示, 代表 质量 块 期 望 的 位 置 ,假定 
轨迹 线 平 滑 ,存在 x (5) ,xx(b ,跟踪 误差 定义 为 e=xs-x, 则 跟踪 此 轨迹 的 伺服 控制 方法 可 
以 给 定 为 

f =xatk,etk,e (8. 60) 

TEX C8. 60) 代 入 式 (8.57) ,得 

x=x;+k,e+k e (8.61) 
或 者 
ek, ek, e z 0 (8. 62) 
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式 (8. 62) 为 伺服 控制 系统 在 误差 空间 下 的 二 阶 线性 微分 方程 ,描述 了 与 期 望 的 轨迹 对 
应 的 误差 的 发 展 。 选 择 合理 的 增益 大 LI, ,可 以 得 出 误差 的 任何 响应 。 轨 迹 跟 踪 控制 方法 框 
图 如 图 8. 25 所 示 。 





图 8.25 一 种 质量 块 轨迹 跟踪 控制 方法 框图 


8.4.2. 机 大 人 计算 力 抱 控制 法 


机 器 人 控制 方法 很 多 ,但 是 大 多 数 都 是 计算 力矩 控制 方法 ( Computed-torque Controllers ) 
的 特例 ,计算 力矩 控制 方法 是 非 线 性 系统 中 线性 反馈 的 一 种 具体 应 用 。 机 器 人 控制 方法 也 
可 以 分 为 类 计算 力矩 控制 方法 或 者 非 计算 力矩 控制 方法 。 类 计算 力矩 控制 方法 也 出 现在 鲁 
棒 控制 、 自 适应 控制 及 其 他 控制 方法 中 。n 自由 度 机 器 人 动力 学 模型 表示 为 
M(g)qg+V(q,q)+F,(q)+F,(q)+G(q)+7,=7 (8. 63) 
简写 为 
M(g)g+N(g,q)+7,=7 (8. 64) 
式 中 ,F,(q) ,F,(gq) 表 示 医 性 摩擦 和 干 摩擦 项 。 从 控制 的 角度 看 问题 ,gq 为 机 器 人 系统 关节 
变量 ,为 系统 的 输出 ;7 为 作用 于 关节 的 力矩 ,为 系统 的 输入 ;r, 为 系统 的 外 部 扰动 ,为 系统 
的 输入 。 利 用 上 述 提 到 的 控制 分 解 方法 ,假定 控制 器 的 输出 为 


7T-aT +B (8. 65) 
R æ e R™,B eR” ,o,B 可 以 选 为 
a-M.B-N (8. 66) 
将 式 (8. 65 ) 代 人 式 (8.64) ,得 
q-1-M' 7, (8.67) 


AUR thia AK HH PD 控制 方法 ,相似 于 式 (8. 60) 对 单位 质量 系统 控制 ,对 单位 惯性 系统 
的 控制 为 


7'-q,*k,e«k,e (8. 68) 
X! ,ezq,-4,k,,k, e R 为 对 角 和 矩阵 。 将 式 (8. 68 ) 代 入 式 (8.67) ,得 
eck,eck,e- M7, (8. 69) 


3k (8. 69 ) 为 对 应 于 nn 个 关节 的 nn 个 解 看 的 关节 跟踪 误差 微分 方程 ,设置 k,,k, 参数 可 
以 改变 机 器 人 系统 每 个 关节 的 控制 性 能 指标 。 如 何 要 如 公式 (8. 68) 般 给 定 新 输入 7', 需 要 
简要 说 明 的 是 , 跟 踊 误 差 相 对 时 间 微 分 两 次 ,得 
e-q,-q (8. 70) 
通过 式 (8. 64) 求 解 4 并 代入 式 (8.70) ,得 
e-q,4M (N«z,-7) (8.71) 
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定义 控制 输入 函数 为 
u-q,*M (CN-7) (8.72) 
扰动 函数 为 
w-M'r, (8. 73) 
在 跟踪 误差 空间 ,定义 状态 矢量 为 
e 
z=| ;| (8. 74) 
式 中 ,x e R” , 则 跟踪 误差 状态 方程 为 
d fel 0 lire 0 0 
aal "lo o dull" V 
式 (8. 72) 所 表示 的 反馈 线性 化 变换 转化 为 
r-M(q,-u) *N (8. 76) 


式 (8.76) 所 表示 的 方法 称 为 计算 力矩 控制 方法 。 式 (8.75) 所 代表 的 系统 为 线性 的 布 
鲁 诺 夫 规 范 型 (Brunovsky Canonical Form) ,系统 的 驱动 为 控制 输入 u(t) 和 扰动 输入 w(i)。 
如 果 我 们 选择 合理 的 控制 输入 w(t) ,可 以 使 式 (8.75) 所 表示 的 系统 为 稳定 系统 , 即 e 趋向 
零 。 根 据 式 (8. 76) ,作用 于 关节 上 的 非 线性 力 7(1) ,将 驱动 关节 跟随 期 望 的 轨迹 。 将 式 
(8.76) 代 入 式 (8. 64) ,得 


M(q)q*N(q,q) *r,- M(q,-u) *N (8. 77) 
进一步 得 
e-u*M ^r, (8.78) 
式 (8.76) 所 表示 的 计算 力矩 控制 方法 ,如 图 8. 26 所 示 。 


r-M(q;-u)*N 





图 8. 26 计算 力矩 控制 方法 

1. PD 外 环 设计 
选择 控制 输入 为 PD 线性 反馈 ,表示 为 

u=-k,e-k,e (8.79) 
作用 于 机 械 臂 关节 力 的 输入 为 

7T=M(g) (q,*k,e*k,e) «N(q,q) (8. 80) 

其 PD 计算 力矩 控制 方法 原理 如 图 8. 27 所 示 。 系 统 闭 环 的 误差 动力 学 方程 为 

e+tk,etk,e=w (8. 81) 
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图 8.27 PD 计算 力矩 控制 方法 原理 图 


系统 特征 多 项 式 为 
A (s)z Is Ek, sk, | (8. 82) 
式 中 ,k,=diag(k,) ,k, 2 diag( k, ) ,得 
AG) = J] + kas tk.) (8.83) 


在 系统 中 , 当 w 有 界 , 合 理 选 择 如 ,入 能 保证 跟踪 误差 趋向 稳定 , 当 M- 存在 上 限 ,7, 有 
界 时 , 则 w 有 界 。 应 该 注意 到 , 当 外 环 设计 时 ,PD 增益 参数 的 和 矩阵, ,k, 在 控制 中 是 解 看 的 ， 
但 整体 的 关节 控制 却 并 不 解 看 ,因为 反馈 信号 u 在 内 环 需 要 乘 以 M 并 与 非 线 性 项 NN 相 加 ， 
二 阶 系 统 特征 多 项 式 的 标准 形式 为 


p(s)=s +2t0@, stw. (8. 84) 
各 个 关节 的 跟踪 误差 的 性 能 可 以 通过 设置 参数 E, ,如 来 调整 ,为 
ki, ki = wn (8.85) 


一 般 而 言 ,靠近 末端 的 关节 e, 比 离 基 座 近 的 关节 设置 大 一 些 , 因 为 机 械 臂 从 基 座 开始 
各 杆 件 的 质量 依次 减少 ,末端 关节 啊 应 应 快 一 些 。 系 统一 般 要 求 为 临界 阻尼 , 即 上 =1, 此 时 
本 (8. 86) 
控制 系统 固有 频率 e, 的 选择 见 8.3. 6 小 节 。 
2. PID 外 环 设 计 
当 外 部 无 扰动 时 ,PD 方案 的 控制 是 有 效 的 , 当 存 在 外 部 扰动 时 ,PD 控制 的 稳 态 误差 非 
零 , 当 采 用 PID 控制 方案 时 ,外 环 控 制 输入 为 


is 8. 87 
| DM) 


作用 于 机 械 臂 关节 力 的 输入 为 
r=M(g) (gutk,etk,e+ke) «N(q,q) (8. 88) 
PID 计算 力矩 控制 方法 原理 图 如 图 8. 28 所 示 。 
闭环 误差 动力 学 方程 为 
e+k,e+k e+k;£=w (8.89) 


e(s)- (s) (8. 90) 


s 
E n N O A 
s Ik, *k sk; 
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图 8.28 PID 计算 力矩 控制 方法 原理 图 


闭环 特征 多 项 式 为 
Es ls Lek, sk, k;| (8.91) 
式 中 ,k, ,大 k; 为 对 角 和 矩阵 。 式 (8. 91 ) 最 终 得 
A(s) = I kgs, T (8.92) 
使 用 Routh-Hurwitz 判 据 ,系统 稳定 的 须 满 足 的 条 件 为 
ks ck, (8. 93) 


[918.5] 2R 机 械 臂 PD 计算 力矩 控制 方法 仿真 ,图 8.29 所 示 2R 机 械 臂 ,各 物理 参数 
如 图 所 示 ,假定 杆 件 质量 集中 于 各 杆 的 末端 ,所 建立 的 动力 学 方程 为 





图 8.29 2R 机 械 臂 杆 件 质量 集中 各 杆 末端 


M(q)q*V(q.q) *G(q)-7 

式 中 
x | ( m, 4m, )a; *m;a; 42m,a, a.c, —— hi 
m,a, *m54à, 05€, m,a, qə 


y- | -M a] a)(241q; tq; "| 


2 
m4G9G5q|$; 


po lu 

,7 — 

m5805C|, 

(1) PD 计算 力矩 控制 方法 。 

如 果 采 用 PD 计算 力矩 控制 方法 ,给 定 为 | 
r-M(q)(q,*k,e«k,e) -V(q,q) *G(q) 


T5 
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(2) 期 望 的 轨迹 。 
关节 1T 2 期 望 的 轨迹 q 给 定 为 
qis=gisin(27it/T) 
qa; 7 g,cos( 2mt/T) 
式 中 ,T=2 s,g;=0.1 rad, Jy TRUE R EE HIER ER ERE , ete 4] IN 2s SERES IR] 2 ( Time Con- 
stant) JJ 0. 1 s, 并 且 系 统 为 临界 阻尼 , 则 各 驱动 关 E Lk, N 
9.170. 1210 
ke, -100 
E; -2«,-20 
(3) Matlab 编程 仿真 。 
在 上 述 参数 已 知 的 前 提 下 ,假设 无 外 部 扰动 ,状态 变量 定义 为 x=[q qq 9;」 ,初始 值 
为 x。=[0.1000] ,通过 Matlab 程序 ,实现 在 此 控制 方法 下 ,关节 实际 的 位 置 ,跟踪 误差 以 
及 作用 于 各 关节 上 的 力矩 的 仿真 ,仿真 结果 如 图 8. 30 所 示 ,仿真 程序 见 附录 下 。 





图 8. 30 2R 机 械 臂 PD 计算 力矩 控制 仿真 图 


本 章 小 结 


本 章 从 一 般 的 二 阶 线性 系统 入 手 ,阐述 了 控制 系统 相关 的 基本 概念 ,重点 讨论 了 机 械 臂 
的 独立 天下 控制 与 计算 力矩 控制 两 种 方法 ,涉及 了 直流 电机 模型 建立 ,并 进一步 建立 了 电 
机 - 轮 系 - 机 械 臂 耦合 的 控制 系统 ,并 采用 了 PD,PID 方法 以 及 前 馈 控 制 方法 ,基于 所 建立 的 
模型 进行 了 控制 仿真 ;详细 阐述 了 计算 力矩 控制 方法 的 原理 ,并 基于 此 方法 ,对 2R 机 械 臂 
进行 了 控制 仿真 ,直观 感受 到 控制 方法 不 同 之 处 ,使 读者 理解 这 些 控制 方法 ,可 以 以 此 为 基 
础 学 习 更 加 深奥 的 控制 方法 ,并 运用 到 实际 工程 中 去 。 


附 x 


附录 A 数学 基础 


附 A.1 天 量 代数 


1. 矢量 点 乘 (Dot Product of Vectors) 
【定义 A.1】 矢量 a 和 矢量 b 点 乘 为 
a*b-zb-*a-lallbicos(a,b) (1) 
如 果 
a=a,ita, j*a,k (2) 
b=b,i+b, j+b,k 
式 中 ,i,j,k 是 相互 垂直 的 单位 矢量 ,得 
a * b=a,b,+a,b,+a,b, (3) 
ij ,k 符合 以 下 约束 条 件 : 
| 
i*J—0,175*E-0,k «150 
任何 一 个 矢量 可 以 表示 为 
y zv,iv, j+v,k (4) 
也 可 以 表示 为 
y-(v*i)i*(v * J)Jo(v - k)k (3) 
式 中 ,vy timo, ,其 他 同 式 (1)。 
如 果 矢 量 a MRE b 以 矩阵 形式 表示 


则 
a-b-alb (7) 
2. 矢量 又 乘 ( Cross Product of Vectors) 
【定义 A.2】 矢量 a 和 矢量 SUE 
axb=-bxa=|lallblsin(a,b)n (8) 
式 中 ,n 是 一 个 单位 矢量 ,其 方向 按照 右手 规则 从 a 转动 (a,5) 角 到 5b, 拇指 所 指 方向 ,同时 
垂直 矢量 a 和 矢量 b。axb 运算 可 以 表示 为 以 下 行列 式 形式 : 
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i j k 
axb-|a, a, a, (9) 
b, b, b 
计算 得 
j k 
a, G,| |a, a, a, a, 
&, Gai 一 +k = 
b, b| |bh 5| |bh b, 
x b, b, 





(a,b,—a,b,)i+(a,b,-a,b,)j+(a,b,-a,b,)k (10) 
矢量 a MRE b UIER , B 


a, b, 
a, b, 


矢量 a 和 矢量 2 又 乘 可 以 表示 为 
c-axb -ab (12) 
式 中 ,a=ax, 是 由 矢量 a 构造 而 成 的 斜 对 称 和 矩阵。 
a 8, [5 
c-ab-|a, 0 | (13) 
x S 0 Jio, 





【定义 A.3】 三 个 矢量 a,b 和 < 的 标量 三 重 积 ( Scalar Triple Product of Three Vectors) 
为 


d, 0, d; 
a*(bxc)-a-* bxc-axb - 4 b, ] (14) 

C 0, 4 
【定义 A.4】 三 个 矢量 a,b 和 e 矢量 三 重 积 (Vector Triple Product of Three Vectors) 为 
ax(bxc)- a * cb-a - bc (15) 

3. 矢量 的 正 交 分 解 (Orthogonal Decomposition ) 
如 图 A. 1 所 示 ,对 于 一 个 矢量 ”进行 正 交 分 解 为 两 个 分 量 , 分 别 平 行 和 垂直 于 单位 矢 
He, 平行 于 单位 矢量 @ 的 分 量 wy 称 为 轴 疝 分 量 ( Axial Component) ,表示 为 


y,-(v-*e)eze(e'v)- ee'v (16) 
垂直 于 e 的 法 向 分 量 v, (Normal Component) ,表示 为 
y, =v-v j = (I-ee! )v (17) 
也 可 以 表示 为 
y, zex(vxe)- (exv) xe (18) 


经 化 简 与 公式 (17) 一 致 。 
附 A.2 四 元 数 代 数 
二 维 平面 的 旋转 可 以 用 复数 来 表达 ,三 维 空间 中 的 旋转 可 以 用 四 元 数 来 表示 ,用 四 元 数 
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图 A.1 矢量 的 正 交 分 解 
表示 三 维 的 旋转 与 使 用 旋转 矩阵 相 比 具有 两 个 优点 :也 几何 意义 明确 ;@ 计 算 简 单 。 此 外 ， 
四 元 数 代 数 还 涵盖 了 矢量 代数 实数 .复数 和 矢量 都 可 以 看 作 是 四 元 数 的 特例 。 四 元 数 是 将 
三 维 矢 量 代 数 推广 到 乘法 和 除法 运算 的 必要 性 而 产生 的 ,四 元 数 由 一 个 实数 单位 和 三 个 虚 
数 单位 i,j,k 组 成 ,通常 把 四 元 数 写 为 一 个 矢量 ,其 一 般 形式 为 
q =qo+qii+qaJ+q3k (19) 
式 中 ,go ,qi1,92 ,93 均 为 实数 ,i,j,k 服从 以 下 运算 规律 ,如 图 A. 2 Brzs ,JSE ET THAI JJ IE , 3 
时 针 为 负 , 即 
六 = 大 = -ji 
jk-i--kj 
ki 2j - -ik 
i =j =K - v 
可 以 看 出 ,如果 q, =q =q =0, 则 四 元 数 退 化 为 实数 ;如 果 q =q =0, 则 四 元 数 退 化 为 复 
数 , 所 以 称 四 元 数 为 超 复 数 。 将 四 元 数 分 为 标量 部 分 9。 和 矢量 部 分 q, 组 成 ,可 以 写 为 
q-749*4, (21) 
AP ,q, 7 q,i*qii*qik , RE 8 RB EEIUEX 
q=|[ go q,] alii qi 4» q3] 


(20) 


"es sg 
图 A.2 乘法 顺序 图 
1. 四 元 素 运 算 ( 加 法 和 乘法 ) 和 性 质 
(OME. 
任 取 两 个 四 元 数 
q7q4,*4,l*q;j*q.k 
Ne (22) 
Donau 
则 
q*p - (qo*po) * (qi *pi) i (q5 9p; )j* (q4*p, )k (23) 


可 以 看 出 ,任意 两 个 四 元 数 相 加 仍 为 四 元 数 , 且 可 以 证 明 四 元 数 加 法 满足 交换 律 和 结合 
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(2) 乘 法 。 
两 个 四 元 数 的 乘法 按 多 项 式 乘法 进行 ,可 以 把 这 种 乘法 叫 作 直 乘 。 
gp=(qotqiitq, j*q4k) (potpiitp, J+psk ) = 
[ gopo—( qipi1+q2p2+q3p3) ] + 
[ doPi *diPo* ( qsp3—q3p2) Ji+ 
[ gop2+q2po+( qap1-—qip3) Jj+ 
L gop3+qapo+( qip2—q2p1) ]k (24) 
四 元 数 乘 四 元 数 仍 为 四 元 数 , 四 元 数 乘法 以 符号 ”* ”表示 。 为 了 运算 方便 ,四 元 数 乘 
法 可 以 有 以 下 两 种 表示 方式 
GD 借用 矢量 运算 符号 表示 法 。 
a a ET 
—-po?P, 
q *p-(qo*q,) * (po+p,)= qopo*qoP, *poq,*4.D, (25) 
式 中 
q,p, 7 (qiitqsJ+qg3k) (piitp j+p;k)= 
—( qıPı +Q2P2+q3P; ) + 
(gap3—qa3p2 )it+( gap1—qip3)J+( qip2—q2Pp1 ) k = 
-q, * D,* (Q,Xp,) 
所 以 
q*p-(qo*q,) * (po+p,)= qopo-4, * P,*qoP,*Poq,* (Q,XD,) (26) 
HT (q,xp,) X (p.xq,) ,所 以 gq «psp * 9q, 即 四 元 数 法 不 符合 交换 律 ,但 可 以 证 明 它 满 
足 结合 律 和 分 配 律 。 可 将 矢量 部 分 的 点 乘 和 又 乘 表 示 为 直 乘 的 组 合 为 


1 
q, “P,=-7 (4 * D,*p, * q,) 


(27) 
1 
q,Xp,-7, (4, * D,-p, * q,) 
根据 式 (27) ,得 出 如 下 结论 : 
d lic "Au ,-0 .* stp, * ,=0 
q, Lp,€»4, * p, -0€4, * p,*p, * q (28) 


q, | P,.94q,Xp, - 0634, * p,-p, * q,-0 
H A, (24 DAP ACROSS ELE TES DIE PRLZTE BEST D 3X SOS AD ,而 在 一 般 情 况 下 
是 不 符合 交换 律 的 。 
DEERE. 
将 四 元 数 写成 四 维 矢量 形 式 
q=lg。 q 4 d] 
p-lp Pi P ml 


do “i Q: h || Po 
qi do 3 4 pi 

q*p- (29) 
do Q do -di || P2 


Q3 2 qı do P3 
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(3) 单 元 、 零 元 和 人 负 四 元 数 。 
单元 为 
I=1+0i+0j+0k 
0=0+0i+0j+0k 
负数 为 
-4 = -q,-4,0-q3j-q.k 
(4) 逆 元 。 
四 元 数 逆 元 以 q^ 表示, 即 
d. 1 B (497410743) -45k) i qo-qii-qaj-qak  q'* — (30) 


gotqiitqgaj+gak (qo*q,i9quj*qsk) (qo7q,i-q,j-q4k) — quqiq54q, N (q) 
式 中 ,gqg* =go-qii-qxj-q3k, 称 为 四 元 数 g WAA% M (q)= | gq 296*0:9;*35 称 为 四 
元 数 的 范 数 ,N(q)= 1q1=Vgqotqi+92+93 表示 四 元 数 的 模 。 若 N(q)= 1, 则 

q -q4' (31) 
B[ 3 758 SET 2E EB UAR ,四 元 数 是 正规 的 。 

(5) 除 法 。 
由 于 乘法 不 可 交换 , 故 除法 分 为 左 除 和 右 除 。 
例如 ,gq,p,x 为 三 个 四 元 数 , 即 

q*x-p,llix-q'*p 

x*q=p, M] x=p * q” 
Bg pp *q”, 故 两 个 x 是 不 相等 的 。 


(6) 四 元 数 的 性 质 。 
db 诸 四 元 数 之 和 的 共 斩 四 元 数 等 于 共 斩 四 元 数 之 和 , 即 
(q*p*x) ' -q' +p* HE (32) 
DH Vu CAI TAB 2c Vu 7c 4C T HESS VU 7628 AA URR , B 
(q*p*x)'-x' *p *q' (33) 
@) 诸 四 元 数 之 积 的 范 数 等 于 其 因子 的 范 数 之 积 , 即 
la*pP*xl-la«al lpl xi (34) 
OH Vu TU ACA RBS , BI 
(q*p*x) -x *p * q (35) 
@) 诸 四 元 数 相 乘 , 当 其 因子 循环 置换 时 ,四 元 数 乘积 的 标量 部 分 不 变 , 即 
Scal(q * p *x)-2 Scal(x *q *p) (36) 


@ 非 循环 置换 时 ,标量 部 分 不 相等 。 
2. 四 元 数 的 几何 意义 和 三 角 表 达 式 
考虑 矢量 的 转动 , 设 矢量 p 旋转 9 角 变 成 矢量 p',1p1=1p'l=7; 设 垂直 于 p,p' 所 在 平面 
的 转动 轴 矢 量 为 e ,转动 方向 与 转轴 方向 遵循 右手 定 则 ,如 图 A. 3 所 示 。 因 为 
p .万 '= 产 cos 0 


pxp' -r/sin be 
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从 而 
p'*p--p':p*p'xp- —r/cos 0-rsin 0e 
进而 
p'=(-r cos 0-r/sin ĝe) «p^! (37) 
根据 式 (30) ,得 
sy — (38) 
将 式 (38) 代 入 式 (37) ,得 
p'=(cos Ot+sin ge) *p (39) 


由 此 可 见 , 形 如 cos 9+sin be 的 四 元 数 表 示 一 个 旋转 :以 e 为 转轴 ,使 垂直 于 转轴 平面 内 
的 一 个 矢量 按照 右手 螺旋 方向 转 过 68 角 。 当 己 反 回旋 转 时 ,定义 角度 为 负 值 ,把 -28 代入 式 
cos b+sin be 中 得 到 cos b+sin 0( —e) ,相当 于 转轴 反问 ,这 与 实际 相符 合 , 而 且 这 两 种 旋转 互 
逆 , 连 续 实施 此 两 个 旋转 相当 于 没 转 ,这 与 cos 09+sin 0e 与 cos 9+sin 0( -e) 乘 积 为 1 相符 。 
利用 式 (39 ) 可 以 方便 实施 一 个 旋转 ,把 表示 旋转 的 四 元 数 左 乘 到 与 e 垂直 的 矢量 上 即 可 。 





图 A.3 四 元 数 的 几何 意义 


对 任意 的 一 个 四 元 数 g=go+g,, 令 p=1g,1=Vgqi+9;+93 ,4,=pe,e X q, 的 单位 矢量 , 即 
由 q, 方向 余弦 组 成 的 矢量 。 


N(q)= 1g1=WV90o+91+92+93 7 A/ 90+D (40) 
利用 三 角 替 代 法 
COS 9=1 sin | 
有 
q=qgo+q,=1g|(cos 0+sin 0e) (41) 


可 见 ,任意 一 个 四 元 数 也 表示 一 个 旋转 , 它 使 矢量 旋转 的 同时 还 使 矢量 的 大 小 变 为 原来 
的 191 倍 ,这 就 是 四 元 数 的 几何 意义 。 

3. 一 般 矢 量 旋转 的 四 元 数 表 示 

首先 令 

p'-q*p*q. (42) 
3X (42) 的 变换 称 为 四 元 数 的 旋转 变换 ,如 果 令 
p =Po +P, 7 po*pil*tpyj*psk 
p'-pytp,7potpiltpsj*psk 
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q-74,*q,0*q;j*qsk 
利用 旋转 变换 式 (42) , RT ELE SI p' 5j p 各 分 量 之 则 的 关系 为 
P'o =Po 
，_go+9g1-92-93 。 2(9g1g2-gog3) ,2(9193+90g,) 
b l 2 
| « |l la |l la |l 
的 „90791+0793 | 2 (0245-4041) (43) 
| 4 || || 4 |l j lg |l : 
y-2nd-qe5), 2nd qun) qo-d dtd 
|| 4 |l lag |l : | «4 |l É 
当 旋 转变 换 四 元 数 是 规范 四 元 数 时 , || q || 2 1, TELLURE SE oC EHE EE PCS d 


3 


时 
p'-q*p*q' (44) 
则 两 个 四 元 数 p' 与 p 矢量 部 分 的 关系 可 以 用 和 矩阵 形式 表示 为 











"P do*41-4;-4.  2(4145;-49433) 2(4:45*394:) | [ p 
p5|-|2(qd145*4943) 4o-4:*4»-45  2(4545-4941) || P (45) 
P's] |2(q45-494;) 2(4.45*4941) 46-41-45 *45 |] LPs 


形式 如 g * () *q' 称 为 旋转 算 子 。 

考虑 一 般 的 绕 指 定 方向 轴线 的 旋转 ,如 图 A. 4 R, RE p 绕 过 0 以 单位 向 量 e 为 方 
加 的 直线 旋转 0 £8 SIG p' ,由 于 回 量 的 平移 不 变性 ,轴线 不 必 固 定 过 点 0。 矢 量 p 可 以 分 解 
为 平行 于 e WREE p MERT e 的 分 量 p,, 即 





图 A.4 绕 指 定 方向 轴线 的 旋转 
P-PTP. (46) 


1 1 
p,-(p-e)e-—,(p*ere*p) cec, ip-e*pe) (47) 
p,-ex(pxe)- Fex(p *e-e*p)- 
{le *(p*e-e*p)-(pxe-exp) *e]- 


y pe p * e) (48) 
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由 式 (46) 、(47) 可 知 
-e*p*e-2p,-p-p;-p, 
这 表示 -e *p *e 是 py 和 p ,反问 后 合成 , 即 -e *p *#e 与 p 关于 e 轴 对 称 。e*p*e 是 
p 在 以 e 为 法 向 的 镜面 反射 后 形成 的 矢量 。 矢 量 p 与 转轴 不 垂直 时 ,p RAAE e 为 方 
向 的 直线 旋转 9 角 后 变 成 p', 可 以 看 成 p 垂直 于 e 的 分 量 p , Z6 e 旋转 9 角 , 然 后 与 pj 合成 
为 p', 即 
p'=py+(cos 0*sin 9e) *p,= 
zl (1+cos 0) p^sin ge * p-sin Op * e+(cos 0-1)e *p e] (49) 
可 见 , 此 时 需要 对 矢量 p [B] BE ERA $6. AAR 8 UU 762 , 1 
p'-(uy*tu,e) *p* (uy-u,e)- 
u;p-ugu,p * e*«usu,e *p-u,e «p*e (50) 


比较 系数 可 得 
uo = 六 | *cos 0) 


I 
üt, == um D 


2 
2 .À1 有 
i, (cos 8-1) 
解 此 方程 得 到 一 组 解 


u — 4 
zii 2 
u E E d 
ii 2 
id 
E NEL 
uU cos, +Sin j€,U -cos--sin-ye [A1 3 


则 p'=u *p*u” ,表示 矢量 p R e 旋转 0 Hapo WRH u 换 为 -wu, 因 (-u)*= 
-4 ”, 故 旋转 算 子 (-z) * ()*(-) 和 2&*()*z& 得 出 相同 的 结果 ,实际 上 ,因为 
-u = cos| 7-5) tsin( 2-5) ( -e) 
2 2 
表示 绕 -e 轴 旋 转 2w-9, 故 结果 相同 。 如 果 规 范 四 元 数 u, 给 出 以 角度 0; 绕 轴 e; 旋转 ,规范 
四 元 数 wu 给 出 以 角度 0; Ze) e; 旋转 , 则 顺序 旋转 u; 和 uw 相应 与 算 子 


uj*u;*()*uj *U -(uj*u;) *() *(u/ *uj) (52) 
因为 u; * u, 也 是 规范 四 元 数 , 如 令 
0 0 
uU, =U; * U;—cos z *sin zer (53) 


则 两 次 顺序 旋转 的 结果 ,可 以 用 角度 0, 绕 e, 一 次 旋转 等 价 。 


人 研究 一 种 特殊 旋转 : 设 w=e, 即 u-cos z sin 7 e, 则 转角 为 m, H. u^ =cos 5j sin 3*7 
-e, 所 以 p'=u*p*u”=-e*p*e, 如 图 (A.4) 所 示 , 变 换 -e * p * e 表示 相对 于 e 轴 对 称 的 


天 量 ,而 变换 e *p *e 给 出 矢量 -e *p *e 相反 的 矢量 , 故 变换 e *p e 给 出 原 矢 量 的 镜 反射 。 
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附录 B 旋转 矩阵 与 四 元 数 参 数 转换 算法 


// 旋 转 矩 阵 与 四 元 数 参 数 转 换 

//MatQuatConvert( RotationMatrix m, Quaternion * q) 
// 判 断 和 矩阵 m 是 否 规范 ,如 果 是 往 下 执行 

If MatIsNorm( m) then 


1 
do* ^V l tr, 725 十 733 


If qo 0 then 
Q1*—73) ~—T23/ 4 go 
22* 一 r13 一 r31/49o 
Q5*—75; 771)/4qo 
else 


qo —0 


1 

gr l +ril 一 r22 773 
1 

d21* ^w l +r2z 一 r33 一 Fl 


1 
135V ] +r33 77,1 772; 


If q, 是 (9 ,9q: ,93 ) 最 大 值 then 
f mP rah FẸ then 
q,*— —4, 
end 
If mE rah FÆ then 
34—43 
End 

else if q, 是 否 是 (9 ,9; ) 中 最 大 值 then 
If. mE ry TAE then 
qiti 
end 
f mP ryh FÆ then 
e Q8 
End 

else 
If m 和 矩阵 中 ms 小 于 零 then 
q1*——4| 
end 


If m 和 矩阵 中 rI FÆ then 


e a 
end 
end 
end 
// 四 元 数 规范 化 
QuatNorm( * q) 


end 
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附录 C 动力 学 基础 知识 


1. 牛顿 第 二 运动 定律 (Newton " s Second Law) 
通常 ,作用 于 点 质量 m 的 力 F 是 位 置 速 度 和 时 间 的 函数 ,根据 牛顿 第 二 运动 定律 ,一 
个 党 质量 质点 的 动力 学 方程 表示 为 


mr 2 ma - F(r ,r ,t) (1) 
式 中 ,r 表示 在 惯性 坐标 系 下 质点 的 位 置 矢量 。 公 式 (1) 可 以 进一步 写 为 


Amr) Aw) (ppt) (2) 


可 见 , 质 点 线 动量 随时 间 的 变化 量 等 于 作用 于 其 上 的 合 外 力 F. 
2. 角 动 量 


如 图 C. 1 所 示 ,质点 的 角 动 量 或 者 动量 和 矩 H 定义 为 
H -rxmv (3) 


在 惯性 坐标 系 101 FAG) 两 侧 对 时 间 微 分 ,并 参考 公式 (2) ,得 


dH dr dv 
i ^4 xmv -rxm 4 77 XF =N 


因此 


dH 
T ua 


式 (4) 表 示 质 点 角 动 量 随时 间 的 变化 量 等 于 作用 于 其 上 的 力矩 N。 





图 C.1 长 方 体 的 惯性 线 量 


3. 刚体 的 惯性 矩阵 
如 图 C. 2 所 示 , 刚 体 可 以 离散 成 一 系列 质点 ,假定 质点 i 的 质量 为 m; ,那么 刚体 的 总 质 
量 以 离散 的 形式 定义 为 
m= m, (5) 
如 果 刚 体 的 密度 函数 在 笛 卡 儿 坐 标 系 下 定义 为 p(x,y,z) , 则 刚体 质量 连续 的 形式 定义 
为 
m = | pdv (6) 
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v 表示 质点 在 三 维 空间 占据 的 区 域 , 在 惯性 坐标 系 10} 下 的 位 置 和 撩 量 为 r, ,相对 于 原点 0, 刚 
体 的 质量 一 阶 和 矩 (Mass First Moment) q, 定义 为 


qo = È nr, = [rov (7) 
刚体 的 质心 在 惯性 坐标 系 下 的 位 置 矢量 为 
qo 
unge (8) 





图 C. 2 
在 惯性 坐标 系 下 ,刚体 的 角 动量 以 离散 的 形式 定义 为 
H, = Èr, X m;V, (9) 
假定 刚体 的 角速度 为 o, MRO) 写 为 
Ho = Y mr, x (c xr,) (10) 
写成 连续 的 形式 为 | 
H,-|rx(oxr)pdv (11) 
公式 (11) ,进一步 写 为 | 
Ho = JrC- P) «pde = | | F- pde] œ (12) 
式 中 ,r = r x 为 斜 对 称 和 矩阵 ,相对 于 原点 0 ,定义 质量 二 阶 矩 (Mass Second Moment) 为 
Io = |PC- pde = f [ (r > r)E - rr pc (13) 


式 中 ,E 为 3 x 3 FABER ES 也 称 为 刚体 转动 惯性 矩阵 或 惯性 张 量 (Inertia Tensor), Sr = 
[x y z] , 式 (13) 可 以 展开 写 为 








y +2 e ege e ug 
1 - | -ay "Uk c-r [pde (14) 
l — Xz = ys x! .y 
刚体 的 转动 惯性 矩阵 可 以 写 为 
ho EL o 
zh L, | (15) 
LOL. d, 


式 中 
L, = [| G7 + Ż)p(x,y,z)dxdydz 
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I, = G + x )p(x,y,z)dxdydz 
2 2 
L, = [[G? + p Goy 2) dxdydz 
L, = 1, = [| C- 0p Guy 2) dxdydz 
Les IK — xz)p(x,y,z)dxdydz 


L, = L, = ||(- y2)p(*,y,z)dxdydz 


I'P „Laoly ,1 分别 为 刚体 对 x 轴 、y 轴 ,z 轴 的 转动 惯量 ,而 1 = 六 ,7 LL. = L, 分 别 是 刚 
体 对 平面 Oxy,Oyz,Ozx 的 惯性 积 。 转 动 惯量 是 刚体 转动 时 的 惯性 度量 ,总 是 正 标 量 ,惯性 积 
可 以 看 成 刚体 不 平衡 性 的 度量 , 它 可 以 是 正 值 ` 负 值 或 零 。 惯 性 积 表征 了 质量 相对 坐标 系 平 
面 分 布 的 不 对 称 性 。 知 为 匀 质 刚体 , 且 垂 直 于 坐标 平面 的 所 有 截面 都 是 对 称 图 形 ,其 惯性 积 
为 零 。 转 动 惯 量 和 矩阵 中 的 元 素 的 数值 依赖 于 参考 点 的 选择 和 所 建立 的 坐标 系 , 对 于 规则 的 、 
简单 几何 体 可 以 通过 体积 积分 计算 得 到 ,对 于 大 部 分 不 规则 的 形状 需要 通过 试验 的 方法 得 
到 。 

【 例 C.1】 图 C.3 所 示 为 匀 质 长 方 体 ,其 长 宽 高 为 a,b 和 c, 在 其 几何 中 心 建立 如 图 C.3 
所 示 的 坐标 系 , 其 密度 函数 为 p(x,y,z) ,计算 其 转动 惯量 矩阵 。 





图 C.3 长 方 体 的 惯性 线 量 


/2 a/2 /2 abc 
I =f | (y? * Z)p(x,y,z)dxdydz =p => (a^ c^) 
ux J 2. J am 12 


同 理 
Lp y t4). =p a az +b*) 

刚体 对 称 ,其 他 惯性 积 为 零 。 

4. 平行 轴 原 理 

S C 点 为 刚体 的 质心 点 , 依 此 为 参考 点 建立 一 个 与 惯性 参考 坐标 系 14} 方向 一 致 的 坐 
标 系 1 C} ,如 图 C.4 所 示 。 在 此 坐标 系 下 ,相对 于 质心 点 C 计算 得 到 的 刚体 转动 惯性 矩阵 为 
I, , 则 相对 于 点 0 其 转动 惯量 矩阵 可 以 表示 为 

I, I; + m[ (rc rc)E -rerec) (16) 

其 推导 过 程 可 参考 公式 (13 ) 。 

5. 转动 惯量 矩阵 变换 

如 果 在 刚体 质心 点 C. 建立 局 部 坐标 系 | C1 ,相对 于 C 点 ,在 坐标 系 1C} 下 ,所 得 到 的 惯 
量 和 矩阵 为 Te ,如 果 相 对 点 不 变 , 惯 量 和 矩阵 变换 到 14} 坐标 系 下 表达 式 为 
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图 C.4 平行 轴 原 理 示 意图 


[Ic], -R [I]. R^ (17) 
其 中 ,R 表示 坐标 系 {C} 相对 于 坐标 系 14} 的 旋转 矩阵 。 
简单 证 明 如 下 :在 坐标 系 |C} 下 ,单位 质点 的 位 置 矢量 "r 变换 到 坐标 系 141} 为 
‘r=R'r (18) 
将 式 (8) 代入 式 (13) 得 到 在 坐标 系 14} 下 的 惯性 矩阵 [I] 为 
[Ic]; = fi (^r -^r)E -^r'r' ]pd» = 


f iC CR) e (Rr) ]E E ( R*r) (R°r)"} pdv = 
f Caen) 。 (Rr) ]E B ( Rr) ( £r" R?) | pdv z 
RI [ [Cr -*r) E -°r°r"]pdv} R" = 


R [Ic] cR" 
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附录 D 单 杆 PD 控制 仿真 


96 Author: yangxiaojun 

% R robot arm Dynamics 

% Newton-Euler equation of motion 

clear; clc; close; 

syms t 

9o mechanism variables 

u2s1sm-—lisg 981; 

% Position vector of mass center 

c = cos(sym('q(t)')); s = sin(sym('q(t)')); 

sb a (Ws (a2) -I[xCyGU: 
% angular velocity and acceleration 

omega = |0 0 diff('q(t)',t) | ; alpha = diff( omega,t) ; 
% Gravity 

G = [0-m*g0]; 

% inertia 

IO  m* a2/3; 

% The PD feedback control law 

gama = 30; beta = 45; qf = pi/3; 

tau = gama * (qf — sym('q(t)')) - beta (diff('q(t)',t)) -*0.5*g*axmsec; 
TOI = [0 0 tau | : 

% the equation of motion 

eq = -IO x alpha + cross( rC,G) + TOI ; 

eqz = eq(3); 

% the equation has to be rewritten as a first order system 
9o (x1 =q and x2 = dq) 

slist = idiff('g(t)',t,2) ,diff('q(t)',t) ,"qCt "p i 
nlist = i'ddq', 'x(2)' , 'x(1)'i ; 

eql = subs( eqz , slist , nlist ) ; 

dxl = sym( 'x(2) ) i 

dx2 = solve( egl,'ddq') ; 

dxldt = char( dx1) ; 

dx2dt = char( dx2 ) ; 

fid = fopen( 'Rrobot. m','w-*') ; 

fprintf( fid ,'function dx = Rrobot(t,x) n') ; 

fprintf( fid,'dx = zeros(2,1) ; Wn') ; 

fprintf ( fid,'dx(1) = x(2);Wn'); 
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fprintf( fid,'dx(2) = '); 

fprintf( fid , dx2dt ) ; 

fprintf( fid, ' ; \n“ ) ; 

fclose( fid ) ; 

96 function in file, not constructing a file as a function 

g = inline( sprintf( '| 96s; 96s]|', dxldt, dx2dt) , 't', 'x'); 

9o define initial conditions 

x0 = [pi/18; 0]; 

% Runge-kutta integral method 

[ts,xs] = odedS(g, 0:0.1:10, x0) ; 

subplot( 1,2,1); 

plot(ts,xs( :,1) * 180/pi,'LineWidth’,1.5),... 

xlabel('t(s)'), ylabel('Nq (deg)'), grid, axis([0, 10, 0, 70]) 
subplot( 1,2,2); 

plot(ts,xs( :,2) * 180/pi,'LineWidth',1.5) ,... 

xlabel('t(s)'), ylabel( 'Vomega (deg/s)') , grid, axis([O, 10, 0, 70]) 
fprintf( "Results Xn') ; fprintf( '5n') ; 

fprintf( ' t(s) q(rad) omega( rad/s) \n’); 

| ts , xs | 
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附录 独立 关 市 控制 仿真 


% Author :yang Xiaojun 
% Discrete PID control for continuous plant 
% qd —— the desired joint input 
%q  ——-the actual joint output 
% xk —— the state vector( q,dq) 
cle; clear all; close all; 
kp = 20; kd = 0.5; ki = 300; 
ts - 0. 001; 26 Sampling time 
xk -zeros(2,1) ; 
elz0;le z0; 
for k = 1:1:1001 
time(k) = (k-1) sts; 
qd(k)= 0.50 * sin(2 * pi * (k-1) » ts) ; 
q(k)= xk(1); 
e = qd(k)-q(k); 
de = (e-e l)/ts; 
le = le + e* ts; 
Tm(k)= kp * e + kd * de + ki * Ie; 
para- Tm(k) ;2% D/A 
tSpan- [0 ts]; 
[ tt, xx ] Zode45 ( '"PlantModel' ,tSpan, xk, [ ] , para) ; 
xk = xx(length(xx),:);96 A/D 
96 Control limit 
if Tn( k) »10.0 
Tm(k)- 10.0; 
end 
if Tn(k) «-10.0 
Tm(k)= -10.0; 
end 
ed së; 
end 
figure(1) ; 
plot( time, qd , 'r' ,time,q,'b--') ; 
xlabel( 'time( s) ') , ylabel('qd,q') ; 
legend( 'qd','q') 
figure(2) ; 
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plot( time ,qd-q,’'r'); 
xlabel( 'time( s) ') , ylabel( 'error') ; 


% 独立 关节 PD+ 前 馈 控制 方法 仿真 
9o Author: Yang Xiaojun 
96 Discrete PD Feedforward controller 
clc; clear all; close all; 
kp 220; kd = 0.5; J = 0.0067, B = 0.10; 
ts=0.001; 96 Sampling time 
xk -zeros(2,1) ; 
e 120; 
for k = 1:1:1001 
time(k) = (k-1) * ts; 
qd(k) = 0.50*sin(2 * pi * (k-1) sts); 
dqd(k) = 0.50 *2 *pi * cos(2 * pi * (k-1) * ts) ; 
ddqd(k) = -0.50 *2 * pi *2 * pi *sin(2 * pi* (k-1) *ts) ; 
q(k) = xk(1); 
dq(k) = xk(2); 
e = qd(k)-q(k); 
de = (e-e l)/ts; 
Tm(k) = kp*e + kd *de + J* ddqd(k) + B * dqd(k) ; 
para- Tm(k) ;2% D/A 
tSpan- [O ts]; 
[ tt, xx | =ode45( 'PlantModel' ,tSpan , xk,[ | , para) ; 
xk = xx(length(xx),:); % A/D 
96 Control limit 
if Tn( k) »10. 0 
Tm(k)= 10.0; 
end 
if Tmn( k) «-10.0 
Tm(k)- -10.0; 
end 
e lze; 
end 
figure(1) ; 
plot( time , qd , 'r' ,time,q,'b') ; 
xlabel( 'time( s) ') ,ylabel('qd,q') ; 
legend( "qd ,'q') 
figure(2) ; 
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plot( time,qd-q, 'r') ; 
xlabel( 'time( s) ') , ylabel( 'error' ) ; 


function dx = PlantModel( t,x , flag , para) 
u-para;J-0.0067;B-0.1;Td = 1; 

dx 2zeros(2,1) ; 

ACID) js 

dx(2) = -(B/J) *x(2) + (1/J) * (u - Td); 


No 


附录 下 PD 计算 力矩 控制 仿真 


96 Author: Yang Xiaojun 
96 PD control scheme without disturbance 
96 main function of simulation 


96 mainCTC. m 


clc 

clear 

ts = 0.001; tx [O ; ts ; 10]; 
m = length(t) ; 


% initial state vector 

x= [0.1000]; 

% output joint angle 

ql = []; 2 = []; 

% tracking error 
egqi-[l;ieq2-2[]l; 

% inputing torque 

torque dl = [ ];torque q2 = [ |]; 
% desired input 

qd = [];adp = [ ];qdpp= []; 
% starting 

for n = 1:m 

[ qd qdp qdpp] = SysInp(t(n)); 
xnaddl = runkut(qd, qdp, qdpp,t(n) ,ts,x) ; 
9o tracking error 

e. ql(n) = qd(1) - x(1); 

e. q2(n) = qd(2) - x2) 

96 ql and q2 

ql(n) = x(1); 

q2(n) = x(2); 

% computing the torque 

tp = ctl(qd, qdp, qdpp, x); 
torque ql(n) = tp(1); 

torque q2(n) = tp(2); 

x = xnaddl; 

end 

subplot (2 ,2 ,1 ) 

plot(t,torque q1,t,torque q2) 
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legend( 'joint 1', 'joint 2 ,2 ) 
title( "Torque inputs’) 

grid on 

subplot (2 ,2 ,2 ) 
plot(t,q1,t,q2) 

legend( 'joint 1', 'joint 2',2) 
title( 'Joint angle outputs’ ) 
grid on 

subplot(2,2,3) 

plot(t,e ql,t,e q2) 

legend( joint 1', 'joint 2',2) 
title( "Tracking errors outputs’ ) 


grid on 


% SysInp. m 

% compute desired trajectory 

% computed-torque control simulation( 2R mechanism ) 
function [ qd qdp qdpp] = SysInp(t) 

per = 2;5g1 = 0.1;g2 = 0.1;fact = 2 * pi/per; 
qd(1) = gl * sin(fact * t) ; 

qd(2) = g2 * cos(fact * t); 

qdp(1) = gl * fact * cos(fact * t) ; 

qdp(2) = -g2 * fact * sin( fact * t) ; 

qdpp(1) = -gl * fact/2 * sin( fact * t); 
qdpp(2) = -g2 * fact/2 * cos( fact * t); 


return 


% ctl. m 

% computed-torque control (2R mechanism) 

% x current state vector 

function tp = ctl( qd, qdp, qdpp, x) 

ep-[];mlz 1;m2 = l;al = 1;232 = l;g = 9.8;kp - 100; kv = 20; 
% compute traking error 

e(1) = qd(1) - x(1); 

e(2) 2 qd(2) - x(2); 

ep(1) = qdp(1) - x(3); 

ep(2) = qdp(2) - x(4); 
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% compution of M, N 

M11 = (ml+m2) *al^2 + m2*a22 + 2 * m2 *al * a2 *cos(x(2)); 

M12 = m2 * a22 + m2 * al * a2 * cos(x(2) ) ; 

M21 = m2 * a272 + m2 * al * a2 * cos(x(2)); 

M22 = m2 * a272; 

N1 = -m2*al *a2 * (2*x(3) *x(4) + x(4)/2) *sin(x(2)); 

N1 = NI + (ml + m2) *g*al *cos(x(1)) + m2 * g * a2 *cos(x(1) + x(2)); 
N2-2m2*al*a2*x(3)/2*sin(x(2)) + m2*g*a2*cos(x(1) +x(2)); 
% computoion of control torque 

sl = qdpp(1) + kv* ep(1) + kp*e(1); 

s2 = qdpp(2) + kv*ep(2) + kp*e(2); 

tl = M11 x* sl + M12 * s2 + Nl; 

t2 = M21 *sl + M22 * s2 + N2; 


tp(1) = tl; 
tp(2) = t; 
return 

6 arm. m 


% robot arm dynamcis subfunction without disturbenc 

9o x current state vector 

9o t input torque 

function xp = arm( x, t) 

ml liue od = mm = ap 9.8; 

% compution of M 

M11 = (ml«m2) *al^2 + m2 * a2/722 + 2 * m2 * al * a2 * cos( x( 2) ) ; 
M12 = m2 * a272 + m2 * al * a2 * cos( x(2)) ; 

M21 = m2 * a272 + m2 * al * a2 * cos( x(2) ) ; 

M22 = m2*a272; 


% inversion of M 


det = M11 * M22 - M12 * M21; 
MIll = M22/det; 
MI12 = -MI12/det; 
MI22 = Mlill/det; 


% nonlinear terms 

N1 = -m2*al *a2 * (2* x(3) *x(4) + x(4)2) *sin(x(2)); 

N1 = NI + (ml + m2) *g*al *cos(x(1)) + m2*g*a2*cos(x(1) + x(2)); 
N22m2*al*a2*x(3)/2*sin(x(2)) + m2*g*a2*cos(x(1) + x(2)); 
% state equation 
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xp(1) = x(3); 

xp(2) = x(4); 

xp(3) = MIII * (-NI & t(1)) + MII2* (-N2 + t(2)); 
xp(4) = MII2 x (-NI +t(1)) + MD2*(-N2 + t(2)); 
return 


% funrunkut. m 

9o main subfunction called by runge-kutta integrator 
function xp = funrunkut( qd, qdp, qdpp,t,x) 

% state vector 

tpt = ctl( qd, qdp, qdpp,x) ; 

xp = amu(x,tpt) ; 

return 


9o Fourth-order RUNGGE-KUTTA INTEGRATION 
function xnaddl = runkut(qd, qdp, qdpp,t,ts,xn) 
O6 ts sample period 

06 xn x(n) state vector 

9o xnaddl x(n-1) state vector 

% temp varibales 

xl = [];xpl = []; 

xpn = funrunkut( qd, qdp, qdpp,t,xn); 

xl(1:4) = xn(1:4) +0.5 x ts * xpn(1:4) ; 
t—-t-c-0.5sts; 

xpl = funrunkut(qd, qdp, qdpp,t,x1) ; 
xpn(1:4) = xpn(1:4) + 2 * xpl(1:4); 

x1(1:4) = xn(1:4) + 0.5 x ts * xpl(1:4) ; 

xpl = funrunkut( qd, qdp, qdpp,t,x1) ; 

xpn( 1:4) = xpn(1:4) + 2* xpl(1:4); 

x1(1:4) = xn(1:4) + ts *xpl(1:4); 

t =t+0.5*ts; 

xpl = funrunkut( qd, qdp, qdpp,t,x1) ; 
xnaddl(1:4) = xn(1:4) + ts*(xpn(1:4) + xpl(1:4) )/6;return 
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